/* EasyCODE V8 */
/* EasyCODE ( 0 
PIC32Flight2.0 */
/* EasyCODE ( 0 
Definitions */
/* EasyCODE ( 0 
Define */
#include <xc.h>
#include <plib.h>
#include <string.h>
#include <math.h>
#include "JUART.h"
#include "JI2C.h"
#include "MPU.h"
#include "HMC5883L.h"
#include "MS5611.h"

// PIC32MX795F512H Configuration Bit Settings
// Configuration Bit settings using the config pragma
// SYSCLK = 80 MHz (8MHz Crystal/ FPLLIDIV * FPLLMUL / FPLLODIV)
// PBCLK = 10 MHz
/* Configuration Bit Settings */
// DEVCFG3
// USERID = No Setting

#pragma config FSRSSEL = PRIORITY_7     // SRS Select (SRS Priority 7)
#pragma config FMIIEN = OFF             // Ethernet RMII/MII Enable (RMII Enabled)
#pragma config FETHIO = OFF             // Ethernet I/O Pin Select (Alternate Ethernet I/O)
#pragma config FCANIO = OFF             // CAN I/O Pin Select (Alternate CAN I/O)
#pragma config FUSBIDIO = OFF           // USB USID Selection (Controlled by Port Function)
#pragma config FVBUSONIO = OFF          // USB VBUS ON Selection (Controlled by Port Function)

// DEVCFG2
#pragma config FPLLIDIV = DIV_2         // PLL Input Divider (2x Divider)
#pragma config FPLLMUL = MUL_20         // PLL Multiplier (20x Multiplier)
#pragma config UPLLIDIV = DIV_12        // USB PLL Input Divider (12x Divider)
#pragma config UPLLEN = OFF             // USB PLL Enable (Disabled and Bypassed)
#pragma config FPLLODIV = DIV_1         // System PLL Output Clock Divider (PLL Divide by 1)

// DEVCFG1
#pragma config FNOSC = PRIPLL           // Oscillator Selection Bits (Primary Osc w/PLL (XT+,HS+,EC+PLL))
#pragma config FSOSCEN = OFF            // Secondary Oscillator Enable (Disabled)
#pragma config IESO = OFF               // Internal/External Switch Over (Disabled)
#pragma config POSCMOD = XT             // Primary Oscillator Configuration (XT osc mode)
#pragma config OSCIOFNC = OFF           // CLKO Output Signal Active on the OSCO Pin (Disabled)
#pragma config FPBDIV = DIV_2           // Peripheral Clock Divisor (Pb_Clk is Sys_Clk/2)
#pragma config FCKSM = CSDCMD           // Clock Switching and Monitor Selection (Clock Switch Disable, FSCM Disabled)
#pragma config WDTPS = PS1              // Watchdog Timer Postscaler (1:1)
#pragma config FWDTEN = OFF             // Watchdog Timer Enable (WDT Disabled (SWDTEN Bit Controls))

// DEVCFG0
//#pragma config DEBUG = OFF             // Background Debugger Enable (Debugger is enabled)
#pragma config ICESEL = ICS_PGx2        // ICE/ICD Comm Channel Select (ICE EMUC2/EMUD2 pins shared with PGC2/PGD2)
#pragma config PWP = OFF                // Program Flash Write Protect (Disable)
#pragma config BWP = OFF                // Boot Flash Write Protect bit (Protection Disabled)
#pragma config CP = OFF                 // Code Protect (Protection Disabled)

#define SYS_FREQ		80000000L //80MHz
#define PB_CLK		40000000L //40MHz

//Define Ports
#define BLED LATGbits.LATG13
#define GLED LATEbits.LATE3
#define OLED LATEbits.LATE2
#define RLED LATAbits.LATA0

#define LED1 LATEbits.LATE0
#define LED2 LATEbits.LATE1
#define LED3 LATGbits.LATG14
#define LED4 LATGbits.LATG12

#define FRONTLED LATEbits.LATE0
#define BACKLED LATEbits.LATE1
#define BACKLED2 LATGbits.LATG14
#define SIDELED LATGbits.LATG12

#define BUZZER LATAbits.LATA9


#define BT_RESET LATBbits.LATB15
#define BT_RTS PORTFbits.RF12 //BTRTS

#define MOTORF	0x4A
#define MOTORB	0x2A
#define MOTORL	0x3A
#define MOTORR	0x1A

#define ToRad(x) (x*0.01745329252)  // *pi/180
#define ToDeg(x) (x*57.2957795131)  // *180/pi

//Delay  25ns = 1
#define delay1ms 		DelayMs(1)
#define delay2ms 		DelayMs(2)
#define delay5ms 		DelayMs(5)
#define delay10ms	 	DelayMs(10)
#define delay25ms	 	DelayMs(25)
#define delay100ms	DelayMs(100)
#define delay200ms	DelayMs(200)
#define delay500ms 	DelayMs(500)
#define delay1s 		DelayMs(1000)
#define delay2s 		DelayMs(2000)
#define delay5s 		DelayMs(5000)
#define delay10s		DelayMs(10000)

#define PI  3.141592654

static unsigned int _excep_code;
static unsigned int _excep_addr;
/* EasyCODE ) */
/* EasyCODE ( 0 
Variables */
//Logging
unsigned int 	Tcount=0;
float			LogA[500];
float			LogB[500];
//float			LogC[100];
//float			LogD[100];

//Voltages
float Vlipo=0;
unsigned short VLIPO=0;
float Current=0;
unsigned short CURRENT=0;
unsigned char N_Cells=0;

//Error
unsigned int E_count=0;
unsigned char E_bat_war=0;
unsigned char E_bat_empty=0;
unsigned char E_RF=0;
unsigned char E_CPU=0;
unsigned char E_code=0;
//LED
unsigned int BACKLED_count=0;

//Time
float Tsload=0;
float TsloadMax=0;

unsigned char High=0;
unsigned char Low=0;
signed short DataHL=0;
/* EasyCODE ( 0 */
/* EasyCODE ( 0 
matrix3f */
/* EasyCODE C */
struct matrix3f
{
        float m11,m12,m13;
        float m21,m22,m23;
        float m31,m32,m33;
};
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
vector3f */
/* EasyCODE C */
struct vector3f
{
        float x,y,z;
};
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE < */
static struct matrix3f rotationMatrix;
static float sintheta, costheta, sinomega, cosomega;
static struct vector3f accNormal, accRotated;
/* EasyCODE > */
/* EasyCODE ) */
/* EasyCODE ( 0 
UART */
//UART
float Data_Interval=0.04;
unsigned short x=0;
unsigned int Labview[60];

unsigned long LabviewOut[60];
unsigned char Data[55];
unsigned char Datapart[10];
unsigned long datahex=0;
float datafloat=0;
unsigned char TRANSMIT=0;
unsigned char sendcount=0;
unsigned char sendsize=0;
unsigned char hekcheck=0;
unsigned char dummy=0;
unsigned char grote=20;



unsigned char UART2start=0;
unsigned char UART2state=0;
unsigned char UART2counter=0;
unsigned char UART2Data=0;
unsigned char UART1state=0;
unsigned char UART1counter=0;
unsigned char UART1Data=0;
unsigned char SendSensor=0;
unsigned char Datamode=0;

//DSMX
unsigned char DSMX[20];
unsigned char DSMX_Throttle=0;
unsigned short DSMXdata=0;
float DSMXdatafloat=0;
unsigned char DSMXcounter=0;
unsigned short Channel=0;

//RF 
unsigned int RemoteCounter=0;

//Wi.232
unsigned char ACK=0;
unsigned char Register=0;
unsigned char PWRMODE=0;
unsigned char DATARATE=0;

//GPS
unsigned char GPSDATA=0;
unsigned char GPS_Data[75];
unsigned char GPSCounter=0;
unsigned char GPS_Start=0;
unsigned char GPS_Ready=0;
unsigned char GPSCRC=0;
/* EasyCODE ) */
/* EasyCODE ( 0 
System Control */
//Angles
float AngleX=0;
float AngleY=0;
float AngleZ=0;
float Altitude=0;
float Speed=0;
float declination=0.733;

float SpeedGain=4;
float AltitudeGain=4;
float LPFGain=0.1;
float AltitudeOld=0;
float AltSpeed=0;

//Control loop
unsigned char Mode=0;
unsigned char Update=0;
signed long Throttle=0;
float ThrottleStick=0;
float AngleLimit=45;
float limit=1500; //Integrator limit
float dt=0;
unsigned int loopcounter=0;
float Dfilter=50;
float DfilterS=50;

float SetpointXspeed=0;
float SetpointYspeed=0;
float SetpointZspeed=0;
signed long MotorLimit=2000;
signed long RPMXF=0;
signed long RPMXB=0;
signed long RPMYL=0;
signed long RPMYR=0;


//GPS Navi
float KrN=2;
float KiN=0.01;
float KdN=4;

float KrNS=0;
float KiNS=0;
float KdNS=0;

float LPFGainD=0.003;

float PvalueDX=0;
float IvalueDX=0;
float DvalueDX=0;
float OldErrorDX=0;
float DFilterDX=0;

float PvalueDY=0;
float IvalueDY=0;
float DvalueDY=0;
float OldErrorDY=0;
float DFilterDY=0;

float PIDDX=0;
float PIDDY=0;


//Altitude

unsigned char Max_Altitude=20;
unsigned char AltitudeHoldOn=0;
unsigned char AltitudeControlOn=0;
float SetpointA=0;
float SetpointAFilter=0;
float SetpointAspeed=0;
float ErrorA=0;
float OldErrorA=0;
float KrA=0.2;
float KiA=0;
float KdA=0;
float DFilterA=0;
float LPFGainA=0.01;
float PvalueA=0;
float IvalueA=0;
float DvalueA=0;


float KrAS=500;
float KiAS=50;
float KdAS=0;

float ErrorAS=0;
float OldErrorAS=0;
float PvalueAS=0;
float IvalueAS=0;
float IvalueAS_Old=0;
float DvalueAS=0;

float DFilterAS=0;
float LPFGainAS=0.01;

signed long PIDA=0;

//Stable Mode X
float SetpointX=0;
float ErrorXS=0;
float OldErrorXS=0;
float KrXS=2;
float KiXS=0;
float KdXS=0;
float PvalueXS=0;
float IvalueXS=0;
float DvalueXS=0;
float IDvalueXS=0;
signed long PIDXS=0;

//Acro Mode X
float ErrorX=0;
float OldErrorX=0;
float KrX=10;
float KiX=80;
float KdX=0;
float PvalueX=0;
float IvalueX=0;
float IvalueXold=0;
float DvalueX=0;
float IDvalueX=0;
signed long PIDX=0;

//Stable Mode Y
float SetpointY=0;
float ErrorYS=0;
float OldErrorYS=0;
float KrYS=4;
float KiYS=0;
float KdYS=0;
float PvalueYS=0;
float IvalueYS=0;
float DvalueYS=0;
float IDvalueYS=0;
signed long PIDYS=0;

//Acromode Y
float ErrorY=0;
float OldErrorY=0;
float KrY=10;
float KiY=80;
float KdY=0;
float PvalueY=0;
float IvalueY=0;
float DvalueY=0;
float IDvalueY=0;
signed long PIDY=0;

//stable mode Z
float SetpointZ=0;
float ErrorZS=0;
float OldErrorZS=0;
float KrZS=4;
float KiZS=0;
float KdZS=0;
float PvalueZS=0;
float IvalueZS=0;
float DvalueZS=0;
signed long PIDZS=0;
float P1=0;
float P2=0;
float P3=0;

//Acro mode Z
float ErrorZ=0;
float OldErrorZ=0;
float KrZ=15;
float KiZ=30;
float KdZ=0;
float PvalueZ=0;
float IvalueZ=0;
float IvalueZ_Old=0;
float IDvalueZ=0;
float DvalueZ=0;
signed long PIDZ=0;
/* EasyCODE ) */
/* EasyCODE ( 0 
Frequency Response */
unsigned char FreqResponse=0;

float SignalS=0;  //Injection Sine
float SignalA=0;  //Injection Sine
float SignalAS=0; //Altitude Speed
float SignalZ=0; //Speed Z
float In=0;
float Out=0;
float Error=0; 

float Frequency=0.1;
float Fstep=0.1;
float StartF=0.05;
float EndF=100;
float N=4;
float Amplitude=500;
float Time=0;

float Re_cos_In=0; //Ur
float Im_sin_In=0;	//Ur
float Re_cos_Out=0; //Y
float Im_sin_Out=0; //Y
float Re_cos_Error=0;
float Im_sin_Error=0; 
float Re_cos_Signal=0; //Y
float Im_sin_Signal=0; //Y

float Finished=0;
float Re_cos_In_send=0; //Ur
float Im_sin_In_send=0;	//Ur
float Re_cos_Out_send=0; //Y
float Im_sin_Out_send=0; //Y
float Re_cos_Error_send=0;
float Im_sin_Error_send=0; 
float Re_cos_Signal_send=0; //Signal
float Im_sin_Signal_send=0; //Signal
/* EasyCODE ) */
/* EasyCODE ( 0 
AHRS */
//#define sampleFreq	363.36f		// sample frequency in Hz
//#define betaDef		0.033f		// 2 * proportional gain IMU Without compass
//#define betaDef		0.05f		// 2 * proportional gain With compass
// Variable definitions

float betastart=1.0f;
float betasetting=0.05f;
volatile float beta = 1.0f;								// 2 * proportional gain (Kp)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame
float g0=0, g1=0, g2=0;
/* EasyCODE ) */
/* EasyCODE ( 0 
BiQuad */
//Filter
float currin   = 0.0;   // x[n]
float lastin   = 0.0;   // x[n-1]
float lastin2  = 0.0;   // x[n-2]
float currout  = 0.0;   // y[n]
float lastout  = 0.0;   // y[n-1]
float lastout2 = 0.0;   // y[n-2]

/*//2nd order 1Hz (fs=425Hz)
float a0=0.000054;
float a1=0.000108;
float a2=0.000054;
float b1=-1.985108;
float b2=0.985325;*/

//2nd order 2.5Hz (fs=425Hz)
float a0=0.000335;
float a1=0.000671;
float a2=0.000335;
float b1=-1.962378;
float b2=0.963719;

/*//2nd order 5Hz (fs=425Hz)
float a0=0.001317;
float a1=0.002634;
float a2=0.001317;
float b1=-1.923510;
float b2=0.928777;*/

/*//2nd order 10Hz (fs=425Hz)
float a0=0.005080;
float a1=0.010160;
float a2=0.005080;
float b1=-1.842483;
float b2=0.862803;
*/
/*//2nd order 5Hz (fs=31.25Hz)
float a0=0.163193;
float a1=0.326385;
float a2=0.163193;
float b1=-0.753537;
float b2=0.406308;
*/

/*//1nd order 5Hz (fs=31.25Hz)
float a0=0.335365;
float a1=0.335365;
float a2=0;
float b1=-0.329271;
float b2=0;*/

/*//2nd order 10Hz (fs=31.25Hz)
float a0=0.490831;
float a1=0.981662;
float a2=0.490831;
float b1=0.586306;
float b2=0.377018;*/
/* EasyCODE ) */
/* EasyCODE ( 0 
Motor */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned short F;
        unsigned short B;
        unsigned short L;
        unsigned short R;
        unsigned char Armed;
        
        unsigned short MaxRpm;
        unsigned short MinRpm;
}Motor;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
Gyro */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        float TEMP;
        float X;
        float Y;
        float Z;
        float Xoffset;
        float Yoffset;
        float Zoffset;
        unsigned char WHO_AM_I;
        unsigned char CONFIG;
}Gyro;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
Acc */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        float TEMP;
        float X;
        float Y;
        float Z;
        float Xoffset;
        float Yoffset;
        float Zoffset;
        
        float Zspeed;
        
        float gX;
        float gY;
        float gZ;
        float gZ_filter;
        float G;
        
        unsigned char SELF_TEST_X;
        unsigned char SELF_TEST_Y;
        unsigned char SELF_TEST_Z;
        unsigned char SELF_TEST_A;
        
        float FT_A_X;
        float FT_A_Y;
        float FT_A_Z;
        float SFT_A_X;
        float SFT_A_Y;
        float SFT_A_Z;
        float SFT_X;
        float SFT_Y;
        float SFT_Z;
        
        unsigned char CONFIG;
}Acc;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
Baro */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned short C1;
        unsigned short C2;
        unsigned short C3;
        unsigned short C4;
        unsigned short C5;
        unsigned short C6;
        unsigned int D1;
        unsigned int D2;
        
        signed int dT;
        signed int TEMP;
        signed long long OFF;
        signed long long SENS;
        signed int P;
        
        float Temperature;
        float Pressure;
        float StartPressure;
        float AltitudeOld;
        float Altitude;
        float AltitudeFilter;
        float AltitudeSpeed;
        
        float StartAltitude;
        unsigned char Timer;
        unsigned short TP;
}Baro;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
Compass */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned char ID_A;
        unsigned char ID_B;
        unsigned char ID_C;
        float X;
        float Y;
        float Z;
        float Xoffset;
        float Yoffset;
        float Zoffset;
        
        float X1, X2, X3;
        float Y1, Y2, Y3;
        float Z1, Z2, Z3;
        
        float Angle;
        
        float S_X;
        float S_Y;
        float S_Z;
}Compass;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
GPS */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned char GPGGA[75];
        unsigned char GPGLL[75];
        unsigned char GPRMC[75];
        unsigned char GPGSV[75];
        unsigned char GPGSA[75];
        unsigned char GPVTG[75];
        
        unsigned char GGACRC;
        unsigned char GLLCRC;
        unsigned char GPRMCCRC;
        unsigned char GPGSVCRC;
        unsigned char GPGSACRC;
        unsigned char GPVTGCRC;
        
        unsigned short CRC;
        
        
        float HomeLat;
        float HomeLong;
        float SetLat;
        float SetLong;
        float Target_angle;
        float Target_distance;
        
        float X_Error;
        float Y_Error;
        
        float Latitude;
        float Longitude;
        float Heading;
        float Speed;
        float Altitude;
        
        float DegLat;
        float DegLon;
        float TimeLat;
        float TimeLon;
        unsigned char N_Sat;
        unsigned char Status;
}GPS;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
DSMX */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned short CH0;
        unsigned short CH1;
        unsigned short CH2;
        unsigned short CH3;
        unsigned short CH4;
        unsigned short CH5;
        unsigned short CH6;
}DSMX_Data;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ) */
/* EasyCODE ( 0 
Functions */
/* EasyCODE < */
//Function declaration
void InitPort(void);
void InitINT(void);
void InitTimers(void);
void InitADC(void);
void DelayMs(unsigned int);
void cleararray(float array[], unsigned int grote );
void cleararraychar(unsigned char array[], unsigned int grote );
void cleararrayint(unsigned int array[], unsigned int grote );

float invSqrt(float x);
void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
void matrixMultiplyVector(const struct matrix3f * a, const struct vector3f * b, struct vector3f * c);

float biquad(float in);

void target(void);
/* EasyCODE > */
/* EasyCODE ) */
/* EasyCODE ) */
/* EasyCODE ( 0 
main */
/* EasyCODE F */
int main(void)
{
    /* EasyCODE ( 0 
    Initialize */
    SYSTEMConfigPerformance(SYS_FREQ);
    mOSCSetPBDIV(OSC_PB_DIV_2);
    delay10ms;
    InitPort();
    InitADC();
    InitINT();
    InitTimers();
    /* EasyCODE - */
    int i;
    int j;
    /* EasyCODE ( 0 
    I2C1 Reset */
    LATAbits.LATA15=1;//SDA
    LATAbits.LATA14=1;//SCL
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    LATAbits.LATA15=0; //SDA
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    LATAbits.LATA15=1;//SDA
    for ( j=0;j<9;j++ )
    {
        LATAbits.LATA14=0;//SCL
        for ( i=0;i<2000;i++ )
        {
            asm("NOP");
        }
        LATAbits.LATA14=1;//SCL
        for ( i=0;i<2000;i++ )
        {
            asm("NOP");
        }
    }
    LATAbits.LATA15=0;//SDA
    for ( i=0;i<2000;i++ )
    {
        asm("NOP");
    }
    LATAbits.LATA15=1;//SDA
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    /* EasyCODE ) */
    /* EasyCODE ( 0 
    I2C2 Reset */
    //int i;
    
    LATAbits.LATA3=1;//SDA
    LATAbits.LATA2=1;//SCL
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    LATAbits.LATA3=0;//SDA
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    LATAbits.LATA3=1;//SDA
    
    //int j;
    for ( j=0;j<9;j++ )
    {
        LATAbits.LATA2=0;//SCL
        for ( i=0;i<2000;i++ )
        {
            asm("NOP");
        }
        LATAbits.LATA2=1;//SCL
        for ( i=0;i<2000;i++ )
        {
            asm("NOP");
        }
    }
    LATAbits.LATA3=0;//SDA
    for ( i=0;i<2000;i++ )
    {
        asm("NOP");
    }
    LATAbits.LATA3=1;//SDA
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    /* EasyCODE ) */
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    EasyCODE_COMMENT_START EasyCODE ( 0 
    I2C3 Reset EasyCODE_COMMENT_END
    //int i;
    
    LATGbits.LATG7=1;//SDA
    LATGbits.LATG8=1;//SCL
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    LATGbits.LATG7=0; //SDA
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    LATGbits.LATG7=1;//SDA
    
    //int j;
    for ( j=0;j<9;j++ )
    {
        LATGbits.LATG8=0;//SCL
        for ( i=0;i<2000;i++ )
        {
            asm("NOP");
        }
        LATGbits.LATG8=1;//SCL
        for ( i=0;i<2000;i++ )
        {
            asm("NOP");
        }
    }
    LATGbits.LATG7=0;//SDA
    for ( i=0;i<2000;i++ )
    {
        asm("NOP");
    }
    LATGbits.LATG7=1;//SDA
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    InitI2C1();//Motors
    InitI2C();
    InitI2C3();
    InitUART2();
    InitUART4();
    InitUART6();
    LATFbits.LATF13=0; //BTCTS
    BT_RESET=1;
    
    cleararrayint(Labview, 60);
    cleararraychar(DSMX, 20);
    cleararraychar(Data, 55);
    /* EasyCODE - */
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    BT_RESET=0;
    delay100ms;
    BT_RESET=1;
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    /* EasyCODE ( 0 
    Reset RPM to Zero */
    Motor.F=0;
    Motor.B=0;
    Motor.L=0;
    Motor.R=0;
    Motor.MaxRpm=8333;
    Motor.MinRpm=1000;
    
    i2c_start();
    i2c_write(MOTORF);
    i2c_write(Motor.F>>8);
    i2c_write((Motor.F&0x00FF));
    i2c_stop();
    
    i2c_start();
    i2c_write(MOTORB);
    i2c_write(Motor.B>>8);
    i2c_write((Motor.B&0x00FF));
    i2c_stop();
    
    i2c_start();
    i2c_write(MOTORL);
    i2c_write(Motor.L>>8);
    i2c_write((Motor.L&0x00FF));
    i2c_stop();
    
    i2c_start();
    i2c_write(MOTORR);
    i2c_write(Motor.R>>8);
    i2c_write((Motor.R&0x00FF));
    i2c_stop();
    /* EasyCODE ) */
    BLED=1;
    /* EasyCODE ( 0 
    Initialize Gyro&Acc */
    I2C2BRG=44;	//400kHz @ 40Mhz Bus Clock //44
    Gyro.WHO_AM_I=MPURead(MPU_RA_WHO_AM_I );
    if ( Gyro.WHO_AM_I!=0x68 )
    {
        while ( 1 )
        {
            RLED^=1;
            delay100ms;
        }
    }
    else
    {
        RLED=0;
    }
    //Sets clock source to gyro reference w/ PLL
    MPUWrite(MPU_RA_PWR_MGMT_1, 0b00000001);
    //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
    MPUWrite(MPU_RA_PWR_MGMT_2, 0x00);
    
    //Sets sample rate to 1000/(1+0) = 1000Hz
    MPUWrite(MPU_RA_SMPLRT_DIV, 0x00);
    
    //MPUWrite(MPU_RA_CONFIG, 0b00000000);//Disable FSync, 256Hz DLPF
    //MPUWrite(MPU_RA_CONFIG, 0b00000001);//Disable FSync, 188Hz DLPF
    //MPUWrite(MPU_RA_CONFIG, 0b00000010);//Disable FSync, 98Hz DLPF
    //MPUWrite(MPU_RA_CONFIG, 0b00000011);//Disable FSync, 42Hz DLPF
    MPUWrite(MPU_RA_CONFIG, 0b00000100);//Disable FSync, 20Hz DLPF 
    
    //Disable gyro self tests, scale of 500 degrees/s
    MPUWrite(MPU_RA_GYRO_CONFIG, 0b00001000);
    
    
    //MPUWrite(MPU_RA_ACCEL_CONFIG, 0b00000000);//Disable accel self tests, scale of +-2g, no DHPF
    MPUWrite(MPU_RA_ACCEL_CONFIG, 0b00001000);//Disable accel self tests, scale of +-4g, no DHPF
    //MPUWrite(MPU_RA_ACCEL_CONFIG, 0b00010000);//Disable accel self tests, scale of +-8g, no DHPF
    //MPUWrite(MPU_RA_ACCEL_CONFIG, 0b00011000);//Disable accel self tests, scale of +-16g, no DHPF
    
    
    //Freefall threshold of |0mg|
    MPUWrite(MPU_RA_FF_THR, 0x00);
    //Freefall duration limit of 0
    MPUWrite(MPU_RA_FF_DUR, 0x00);
    //Motion threshold of 0mg
    MPUWrite(MPU_RA_MOT_THR, 0x00);
    //Motion duration of 0s
    MPUWrite(MPU_RA_MOT_DUR, 0x00);
    //Zero motion threshold
    MPUWrite(MPU_RA_ZRMOT_THR, 0x00);
    //Zero motion duration threshold
    MPUWrite(MPU_RA_ZRMOT_DUR, 0x00);
    //Disable sensor output to FIFO buffer
    MPUWrite(MPU_RA_FIFO_EN, 0x00);
    //Setup INT pin and AUX I2C pass through
    MPUWrite(MPU_RA_INT_PIN_CFG, 0b00000010);
    //Enable data ready interrupt
    MPUWrite(MPU_RA_INT_ENABLE, 0x01);
     
    //Slave out, dont care
    MPUWrite(MPU_RA_I2C_SLV0_DO, 0x00);
    MPUWrite(MPU_RA_I2C_SLV1_DO, 0x00);
    MPUWrite(MPU_RA_I2C_SLV2_DO, 0x00);
    MPUWrite(MPU_RA_I2C_SLV3_DO, 0x00);
    //More slave config
    MPUWrite(MPU_RA_I2C_MST_DELAY_CTRL, 0x00);
    //Reset sensor signal paths
    MPUWrite(MPU_RA_SIGNAL_PATH_RESET, 0x00);
    //Motion detection control
    MPUWrite(MPU_RA_MOT_DETECT_CTRL, 0x00);
    //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
    MPUWrite(MPU_RA_USER_CTRL, 0x00);
    
    //Data transfer to and from the FIFO buffer
    MPUWrite(MPU_RA_FIFO_R_W, 0x00);
    /* EasyCODE - */
    Acc.CONFIG=MPURead(MPU_RA_ACCEL_CONFIG);
    if ( Acc.CONFIG!=0b00001000 )
    {
        while ( 1 )
        {
            RLED^=1;
            delay100ms;
        }
    }
    else
    {
        RLED=0;
    }
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    EasyCODE_COMMENT_START EasyCODE ( 0 
    Acc Self Test EasyCODE_COMMENT_END
    Acc.SELF_TEST_X=MPURead(MPU_RA_SELF_TEST_X);
    Acc.SELF_TEST_Y=MPURead(MPU_RA_SELF_TEST_Y);
    Acc.SELF_TEST_Z=MPURead(MPU_RA_SELF_TEST_Z);
    Acc.SELF_TEST_A=MPURead(MPU_RA_SELF_TEST_A);
    Acc.SELF_TEST_A=(Acc.SELF_TEST_A&0b00111111);
    
    Acc.SELF_TEST_X=(Acc.SELF_TEST_X>>5);
    Acc.SELF_TEST_Y=(Acc.SELF_TEST_Y>>5);
    Acc.SELF_TEST_Z=(Acc.SELF_TEST_Z>>5);
    
    Acc.SELF_TEST_X=(Acc.SELF_TEST_X<<2);
    Acc.SELF_TEST_Y=(Acc.SELF_TEST_Y<<2);
    Acc.SELF_TEST_Z=(Acc.SELF_TEST_Z<<2);
    
    Acc.SELF_TEST_X+=((Acc.SELF_TEST_A&0b00110000)>>4);
    Acc.SELF_TEST_Y+=((Acc.SELF_TEST_A&0b00001100)>>2);
    Acc.SELF_TEST_Z+=((Acc.SELF_TEST_A&0b00000011));
    if ( Acc.SELF_TEST_X==0 )
    {
        Acc.FT_A_X=0;
    }
    else
    {
        Acc.FT_A_X=4096*0.34*pow((0.92/0.34), ((Acc.SELF_TEST_X-1)/(pow(2,5)-2)));
        EasyCODE_COMMENT_START EasyCODE < EasyCODE_COMMENT_END
        Acc.FT_A_X=4096*0.34*pow(2706, ((Acc.SELF_TEST_X-1)/31));
        EasyCODE_COMMENT_START EasyCODE > EasyCODE_COMMENT_END
    }
    if ( Acc.SELF_TEST_Y==0 )
    {
        Acc.FT_A_Y=0;
    }
    else
    {
        Acc.FT_A_Y=4096*0.34*pow((0.92/0.34), ((Acc.SELF_TEST_Y-1)/(pow(2,5)-2)));
        EasyCODE_COMMENT_START EasyCODE < EasyCODE_COMMENT_END
        Acc.FT_A_Y=4096*0.34*pow(2706, ((Acc.SELF_TEST_Y-1)/31));
        EasyCODE_COMMENT_START EasyCODE > EasyCODE_COMMENT_END
    }
    if ( Acc.SELF_TEST_Z==0 )
    {
        Acc.FT_A_Z=0;
    }
    else
    {
        Acc.FT_A_Z=4096*0.34*pow((0.92/0.34), ((Acc.SELF_TEST_Z-1)/(pow(2,5)-2)));
        EasyCODE_COMMENT_START EasyCODE < EasyCODE_COMMENT_END
        Acc.FT_A_Z=4096*0.34*pow(2706, ((Acc.SELF_TEST_Z-1)/31));
        EasyCODE_COMMENT_START EasyCODE > EasyCODE_COMMENT_END
    }
    MPUWrite(MPU_RA_ACCEL_CONFIG, 0b11110000);
    delay10ms;
    MPU(&Gyro.X, &Gyro.Y, &Gyro.Z, &Acc.X, &Acc.Y, &Acc.Z, &Gyro.TEMP);
    Acc.SFT_A_X=((Acc.X*16384)/4096);
    Acc.SFT_A_Y=((Acc.Y*16384)/4096);
    Acc.SFT_A_Z=((Acc.Z*16384)/4096);
    
    MPUWrite(MPU_RA_ACCEL_CONFIG, 0b00010000);
    delay10ms;
    MPU(&Gyro.X, &Gyro.Y, &Gyro.Z, &Acc.X, &Acc.Y, &Acc.Z, &Gyro.TEMP);
    Acc.SFT_A_X-=((Acc.X*16384)/4096);
    Acc.SFT_A_Y-=((Acc.Y*16384)/4096);
    Acc.SFT_A_Z-=((Acc.Z*16384)/4096);
    
    Acc.SFT_X=((Acc.SFT_A_X-Acc.FT_A_X)/Acc.FT_A_X);
    Acc.SFT_Y=((Acc.SFT_A_Y-Acc.FT_A_Y)/Acc.FT_A_Y);
    Acc.SFT_Z=((Acc.SFT_A_Z-Acc.FT_A_Z)/Acc.FT_A_Z);
    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
    //Disable accel self tests, scale of +-2g, no DHPF
    MPUWrite(MPU_RA_ACCEL_CONFIG, 0x00);
    EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    BUZZER=1;
    delay100ms;
    BUZZER=0;
    delay2s;
    for ( x=0; x<=1000; x++ )
    {
        MPU(&Gyro.X, &Gyro.Y, &Gyro.Z, &Acc.X, &Acc.Y, &Acc.Z, &Gyro.TEMP);
        Gyro.Xoffset+=Gyro.X;
        Gyro.Yoffset+=Gyro.Y;
        Gyro.Zoffset+=Gyro.Z;
        delay2ms;
    }
    Gyro.Xoffset=(Gyro.Xoffset/1000);
    Gyro.Yoffset=(Gyro.Yoffset/1000);
    Gyro.Zoffset=(Gyro.Zoffset/1000);
    /* EasyCODE - */
    BUZZER=1;
    delay100ms;
    BUZZER=0;
    /* EasyCODE ) */
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    Acc.Xoffset=0;
    Acc.Yoffset=0;
    Acc.Zoffset=0;
    EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    Acc.Xoffset=0.011340387;
    Acc.Yoffset=-0.021658282;
    Acc.Zoffset=-0.049415402;
    /* EasyCODE - */
    GLED=1;
    /* EasyCODE ( 0 
    Initialize Compass */
    HMC5883LWrite(HMC5883L_RA_MODE , 0x00);
    Compass.ID_A=HMC5883LRead(HMC5883L_RA_ID_A);
    Compass.ID_B=HMC5883LRead(HMC5883L_RA_ID_B);
    Compass.ID_C=HMC5883LRead(HMC5883L_RA_ID_C);
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    EasyCODE_COMMENT_START EasyCODE ( 0 
    Compass ID check EasyCODE_COMMENT_END
    if ( Compass.ID_A!='H' )
    {
        while ( 1 )
        {
            RLED^=1;
            delay100ms;
        }
    }
    else
    {
        if ( Compass.ID_B!='4' )
        {
            while ( 1 )
            {
                RLED^=1;
                delay100ms;
            }
        }
        else
        {
            if ( Compass.ID_C!='3' )
            {
                while ( 1 )
                {
                    RLED^=1;
                    delay100ms;
                }
            }
            else
            {
                RLED=0;
            }
        }
    }
    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    /* EasyCODE ( 0 
    Compass Self test */
    HMC5883LWrite(HMC5883L_RA_CONFIG_A , 0b01110001);//8-avg 15Hz Positive Gain
    HMC5883LWrite(HMC5883L_RA_CONFIG_B , 0b01000000);//820(LSB/Gaus)
    HMC5883LWrite(HMC5883L_RA_MODE , 0x01); //Single mode
    delay100ms;
    HMC5883L(&Compass.S_X, &Compass.S_Y, &Compass.S_Z);
    Compass.S_X=((Compass.S_X*1090)/820);
    Compass.S_Y=((Compass.S_Y*1090)/820);
    Compass.S_Z=((Compass.S_Z*1090)/820);
    RLED=0;
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    if ( abs(abs(Compass.S_X)-1.16)>0.5 )
    {
        while ( 1 )
        {
            RLED^=1;
            delay100ms;
        }
    }
    else
    {
        if ( abs(abs(Compass.S_Y)-1.16)>0.5 )
        {
            while ( 1 )
            {
                RLED^=1;
                delay100ms;
            }
        }
        else
        {
            if ( abs(abs(Compass.S_Z)-1.08)>0.5 )
            {
                while ( 1 )
                {
                    RLED^=1;
                    delay100ms;
                }
            }
            else
            {
                RLED=0;
            }
        }
    }
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    RLED=0;
    /* EasyCODE ) */
    HMC5883LWrite(HMC5883L_RA_CONFIG_A , 0b01110100);
    HMC5883LWrite(HMC5883L_RA_CONFIG_B , 0b00100000);
    HMC5883LWrite(HMC5883L_RA_MODE , 0x00);
    delay100ms;
    HMC5883L(&Compass.X, &Compass.Y, &Compass.Z);
    /* EasyCODE - */
    Compass.ID_A=HMC5883LRead(HMC5883L_RA_CONFIG_A);
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    if ( Compass.ID_A!=0b01110100 )
    {
        while ( 1 )
        {
            RLED^=1;
            delay100ms;
        }
    }
    else
    {
        RLED=0;
    }
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    /* EasyCODE ) */
    Compass.Xoffset=0.0335;
    Compass.Yoffset=-0.113;
    Compass.Zoffset=-0.009;
    
    Compass.X1=1.015;
    Compass.X2=0.0063;
    Compass.X3=0.0189;
    
    Compass.Y1=0.0063;
    Compass.Y2=0.9711;
    Compass.Y3=-0.0003;
    
    Compass.Z1=0.0189;
    Compass.Z2=-0.0003;
    Compass.Z3=1.0149;
    /* EasyCODE - */
    OLED=1;
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    EasyCODE_COMMENT_START EasyCODE ( 0 
    Initialize Baro EasyCODE_COMMENT_END
    MS5611Command(MS5611_RESET);
    delay10ms;
    Baro.C1=MS5611Read(MS5611_PROM_COEF1);
    Baro.C2=MS5611Read(MS5611_PROM_COEF2);
    Baro.C3=MS5611Read(MS5611_PROM_COEF3);
    Baro.C4=MS5611Read(MS5611_PROM_COEF4);
    Baro.C5=MS5611Read(MS5611_PROM_COEF5);
    Baro.C6=MS5611Read(MS5611_PROM_COEF6);
    
    MS5611Command(MS5611_D1_OSR_256);
    delay10ms;
    MS5611(&Baro.D1);
    MS5611Command(MS5611_D2_OSR_256);
    delay10ms;
    MS5611(&Baro.D2);
    Baro.StartAltitude=1000;
    EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
    Baro.dT=(Baro.D2-Baro.C5*256);
    Baro.TEMP=2000+(Baro.dT*(Baro.C6/powf(2, 23)));
    
    Baro.OFF=(long long)Baro.C2*65536+((long long)Baro.C4*(long long)Baro.dT)/128;
    Baro.SENS=(long long)Baro.C1*32768+((long long)Baro.C3*(long long)Baro.dT)/256;
    Baro.P=(Baro.D1*Baro.SENS/powf(2, 21)-Baro.OFF)/32768;
    Baro.Pressure=Baro.P;
    Baro.Pressure/=100;
    Baro.Temperature=Baro.TEMP;
    Baro.Temperature/=100;
    Baro.Altitude=44330*(1-powf((Baro.Pressure/1013.25),(1/5.255)));
    if ( Baro.Altitude<Baro.StartAltitude )
    {
        Baro.StartAltitude=Baro.Altitude;
        Baro.StartPressure=Baro.Pressure;
    }
    Baro.Altitude-=Baro.StartAltitude;
    Baro.AltitudeFilter=0;
    Baro.AltitudeSpeed=0;
    Baro.TP=0;
    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    RLED=1;
    /* EasyCODE ( 0 
    GPS */
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    TUART6_Text("$PMTK251,115200*1F");
    delay10ms;
    U6BRG=21; //115200Baud
    delay1s;
    TUART6_Text("$PMTK300,200,0,0,0,0*2F");
    delay1s;
    TUART6_Text("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28");
    delay10ms;
    TUART6_Text("$PMTK301,2*2E");
    delay10ms;
    EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    U6BRG=21; //115200Baud
    /* EasyCODE ) */
    /* EasyCODE ( 0 
    Battery */
    Vlipo=ADC1BUF0;
    Vlipo*=0.0637441406;
    if ( Vlipo>12.75 )
    {
        N_Cells=4;
    }
    else
    {
        if ( Vlipo>9 )
        {
            N_Cells=3;
        }
    }
    /* EasyCODE ) */
    BLED=0;
    GLED=0;
    RLED=0;
    OLED=0;
    BUZZER=1;
    delay500ms;
    BUZZER=0;
    Update=0;
    INTEnableSystemMultiVectoredInt();
    /* EasyCODE ) */
    while ( 1 )
    {
        if ( Update==1 )
        {
            /* EasyCODE ( 0 
            UART Data */
            switch ( Datamode )
            {
            case 0 :
                /* EasyCODE ( 0 
                Control */
                if ( Labview[17]=='*' && grote==17 )
                {
                    /* EasyCODE_STRUCTURED_COMMENT_START */
                    /*
                    //Control Mode
                    //Mode=(Labview[0]);
                    
                    //Setpoints
                    EasyCODE_COMMENT_START datahex=((Labview[1]<<24)+(Labview[2]<<16)+(Labview[3]<<8)+(Labview[4]));
                    SetpointX=*(float*)&datahex;
                    
                    datahex=((Labview[5]<<24)+(Labview[6]<<16)+(Labview[7]<<8)+(Labview[8]));
                    SetpointY=*(float*)&datahex;
                    
                    datahex=((Labview[9]<<24)+(Labview[10]<<16)+(Labview[11]<<8)+(Labview[12]));
                    SetpointZ=*(float*)&datahex;
                    
                    //Throttle
                    datahex=((Labview[13]<<8)+(Labview[14]));
                    Throttle=datahex;
                    
                    //ArmMotor
                    Motor.Armed=Labview[15]; EasyCODE_COMMENT_END
                    
                    //Data
                    SendSensor=Labview[16];
                    
                    Labview[17]=0;
                    grote=0;
                    //ThrottleStick=0;
                    EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                    //Setpoints
                    datahex=((Labview[1]<<24)+(Labview[2]<<16)+(Labview[3]<<8)+(Labview[4]));
                    LPFGain=*(float*)&datahex;
                    
                    datahex=((Labview[5]<<24)+(Labview[6]<<16)+(Labview[7]<<8)+(Labview[8]));
                    SpeedGain=*(float*)&datahex;
                    
                    datahex=((Labview[9]<<24)+(Labview[10]<<16)+(Labview[11]<<8)+(Labview[12]));
                    AltitudeGain=*(float*)&datahex;
                    EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                    */
                    /* EasyCODE_STRUCTURED_COMMENT_END */
                    //Data
                    SendSensor=Labview[16];
                    
                    Labview[17]=0;
                    grote=0;
                }
                /* EasyCODE ) */
                /* EasyCODE ( 0 
                Data Out */
                switch ( SendSensor )
                {
                case 0 :
                    /* EasyCODE ( 0 
                    0:Battery */
                    if ( loopcounter>=((unsigned int)(0.5/dt)) )
                    {
                        //Battery
                        datahex=*(unsigned long*)&Vlipo;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=4;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 1 :
                    /* EasyCODE ( 0 
                    1:Acc.XYZ */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datahex=*(unsigned long*)&Acc.X;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Acc.Y;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Acc.Z;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Acc.gX;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Acc.gY;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Acc.gZ;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 2 :
                    /* EasyCODE ( 0 
                    2:Acc.XYZ Offset */
                    if ( TRANSMIT==0 )
                    {
                    }
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&Acc.Xoffset;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Acc.Yoffset;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Acc.Zoffset;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=12;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 3 :
                    /* EasyCODE ( 0 
                    3:Gyro.XYZ */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&Gyro.X;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Gyro.Y;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Gyro.Z;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        //Angles
                        datahex=*(unsigned long*)&SetpointXspeed;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&SetpointYspeed;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&SetpointZspeed;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 4 :
                    /* EasyCODE ( 0 
                    4:Gyro.XYZ Offset */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&Gyro.Xoffset;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Gyro.Yoffset;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Gyro.Zoffset;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=12;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 5 :
                    /* EasyCODE ( 0 
                    5:AngleXYZ */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&AngleX;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&AngleY;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&AngleZ;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        //Angles
                        datahex=*(unsigned long*)&SetpointX;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&SetpointY;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&SetpointZ;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 6 :
                    /* EasyCODE ( 0 
                    6:PID SpeedX */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datafloat=SetpointXspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Gyro.X;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=ErrorXS;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=PIDX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        datafloat=Motor.L; //Front Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Motor.F; //Front Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=Motor.B; //Back Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=Motor.R;//Back Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 7 :
                    /* EasyCODE ( 0 
                    7:PID SpeedY */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datafloat=SetpointYspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Gyro.Y;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=ErrorYS;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=PIDY;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        datafloat=Motor.L; //Front Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Motor.F; //Front Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=Motor.B; //Back Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=Motor.R;//Back Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 8 :
                    /* EasyCODE ( 0 
                    8:PID SpeedZ */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datafloat=SetpointZspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Gyro.Z;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=ErrorZS;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=PIDZ;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        datafloat=Motor.L; //Front Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Motor.F; //Front Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=Motor.B; //Back Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=Motor.R;//Back Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 9 :
                    /* EasyCODE ( 0 
                    9:PID AngleX */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datafloat=SetpointX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=AngleX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=ErrorX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=SetpointXspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        datafloat=Motor.L; //Front Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Motor.F; //Front Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=Motor.B; //Back Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=Motor.R;//Back Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 10 :
                    /* EasyCODE ( 0 
                    10:PID AngleY */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datafloat=SetpointY;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=AngleY;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=ErrorY;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=SetpointYspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        datafloat=Motor.L; //Front Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Motor.F; //Front Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=Motor.B; //Back Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=Motor.R;//Back Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 11 :
                    /* EasyCODE ( 0 
                    11:PID AngleZ */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datafloat=SetpointZ;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=AngleZ;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=ErrorZ;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=SetpointZspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        datafloat=Motor.L; //Front Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Motor.F; //Front Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=Motor.B; //Back Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=Motor.R;//Back Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 12 :
                    /* EasyCODE ( 0 
                    12:PID Cascade X+Y */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datafloat=SetpointX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=AngleX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=SetpointXspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=Gyro.X;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        datafloat=SetpointY;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=AngleY;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=SetpointYspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=Gyro.Y;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 13 :
                    /* EasyCODE ( 0 
                    13:PID Cascade AngleX+Z */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datafloat=SetpointX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=AngleX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=SetpointXspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=Gyro.X;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datafloat=PIDX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=(SetpointZspeed*-1);
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=Motor.F;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=Motor.B;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 14 :
                    /* EasyCODE ( 0 
                    14:PID SpeedX */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datafloat=SetpointXspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Gyro.X;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=PvalueX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=IvalueX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        datafloat=Motor.L; //Front Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Motor.F; //Front Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=Motor.B; //Back Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=Motor.R;//Back Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 15 :
                    /* EasyCODE ( 0 
                    15:PID X */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datafloat=SetpointX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=AngleX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=PvalueXS;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=DvalueXS;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        datafloat=SetpointXspeed; //Front Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Gyro.X; //Front Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=PvalueX; //Back Right
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=IvalueX;//Back Left
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 16 :
                    break;
                case 17 :
                    break;
                case 18 :
                    /* EasyCODE ( 0 
                    15:Motor */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datafloat=Motor.F;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Motor.B;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=Motor.L;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=Motor.R;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=16;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 19 :
                    /* EasyCODE ( 0 
                    19:PID Altitude */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&ErrorA;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&PvalueA;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&IvalueA;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&DvalueA;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=16;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 20 :
                    /* EasyCODE ( 0 
                    20:Compass.XYZ */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&Compass.X;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Compass.Y;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Compass.Z;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Compass.Angle;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&AngleZ;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=20;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 21 :
                    /* EasyCODE ( 0 
                    21:Baro */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&Baro.Temperature;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Baro.Pressure;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Baro.Altitude;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=12;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 22 :
                    /* EasyCODE ( 0 
                    22:Altitude test */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datafloat=Acc.gZ_filter;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Baro.Altitude;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=Speed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=Baro.AltitudeSpeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=16;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 23 :
                    /* EasyCODE ( 0 
                    23: Altitude velocity PID */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datafloat=SetpointAspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Speed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=PvalueAS;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=IvalueAS;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datafloat=PIDA;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Altitude;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    /* EasyCODE_STRUCTURED_COMMENT_START */
                    /*
                    EasyCODE_COMMENT_START EasyCODE ( 0 
                    23: Altitude velocity PID EasyCODE_COMMENT_END
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datafloat=SetpointAspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Speed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=PIDA;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=Altitude;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        sendcount=0;
                        sendsize=16;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                    EasyCODE_COMMENT_START EasyCODE ( 0 
                    23: Altitude velocity PID EasyCODE_COMMENT_END
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datafloat=SetpointAspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Speed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=PvalueAS;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=IvalueAS;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        sendcount=0;
                        sendsize=16;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                    */
                    /* EasyCODE_STRUCTURED_COMMENT_END */
                    break;
                case 24 :
                    /* EasyCODE ( 0 
                    24:PID Altitude */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&SetpointAFilter;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Altitude;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&ErrorA;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=(float)PIDA;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=16;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 25 :
                    /* EasyCODE ( 0 
                    25:Altitude All */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datafloat=SetpointAspeed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=Speed;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=PIDA;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=SetpointX;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datafloat=Acc.gZ_filter;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=Altitude;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datafloat=Baro.Altitude;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=28;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 30 :
                    /* EasyCODE ( 0 
                    30:Setpoints */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&SetpointX;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&SetpointY;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&SetpointZ;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=(Throttle+ThrottleStick);
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=16;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 31 :
                    /* EasyCODE ( 0 
                    31:DSMX */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Speed Settings
                        datafloat=DSMX_Data.CH0;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datafloat=DSMX_Data.CH1;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=DSMX_Data.CH2;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datafloat=DSMX_Data.CH3;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datafloat=DSMX_Data.CH4;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=DSMX_Data.CH5;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                /* EasyCODE_STRUCTURED_COMMENT_START */
                /*
                case 50 :
                    EasyCODE_COMMENT_START EasyCODE ( 0 
                    50:GPS EasyCODE_COMMENT_END
                    if ( loopcounter>=((unsigned int)(1/dt)) )
                    {
                        datahex=*(unsigned long*)&GPS.Latitude;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Longitude;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Altitude;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Speed;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datafloat=GPS.N_Sat;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datafloat=GPS.Status;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                    break;
                case 51 :
                    EasyCODE_COMMENT_START EasyCODE ( 0 
                    51:GPS Navi EasyCODE_COMMENT_END
                    if ( loopcounter>=((unsigned int)(1/dt)) )
                    {
                        datahex=*(unsigned long*)&GPS.Latitude;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Longitude;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.SetLat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.SetLong;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Target_distance;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Target_angle;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&PIDDX;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&PIDDY;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        sendcount=0;
                        sendsize=32;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                    break;
                */
                /* EasyCODE_STRUCTURED_COMMENT_END */
                case 52 :
                    /* EasyCODE ( 0 
                    52:GPS PID Navi */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //1
                        datahex=*(unsigned long*)&GPS.X_Error;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        //2
                        datahex=*(unsigned long*)&PIDDX;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        //3
                        datahex=*(unsigned long*)&GPS.Y_Error;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        //4
                        datahex=*(unsigned long*)&PIDDY;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        //5
                        datahex=*(unsigned long*)&GPS.Speed;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        //6
                        datafloat=GPS.Target_distance;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 53 :
                    /* EasyCODE ( 0 
                    53:GPS Data */
                    if ( loopcounter>=((unsigned int)(0.2/dt)) )
                    {
                        datahex=*(unsigned long*)&GPS.Latitude;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Longitude;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.HomeLat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.HomeLong;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.SetLat;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.SetLong;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Speed;
                        LabviewOut[24]=((datahex&0xFF000000)>>24);
                        LabviewOut[25]=((datahex&0x00FF0000)>>16);
                        LabviewOut[26]=((datahex&0x0000FF00)>>8);
                        LabviewOut[27]= (datahex&0x000000FF);
                        
                        datafloat=GPS.N_Sat;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[28]=((datahex&0xFF000000)>>24);
                        LabviewOut[29]=((datahex&0x00FF0000)>>16);
                        LabviewOut[30]=((datahex&0x0000FF00)>>8);
                        LabviewOut[31]= (datahex&0x000000FF);
                        
                        datafloat=GPS.Status;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[32]=((datahex&0xFF000000)>>24);
                        LabviewOut[33]=((datahex&0x00FF0000)>>16);
                        LabviewOut[34]=((datahex&0x0000FF00)>>8);
                        LabviewOut[35]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&AngleZ;
                        LabviewOut[36]=((datahex&0xFF000000)>>24);
                        LabviewOut[37]=((datahex&0x00FF0000)>>16);
                        LabviewOut[38]=((datahex&0x0000FF00)>>8);
                        LabviewOut[39]= (datahex&0x000000FF);
                        
                        datafloat=GPS.Heading;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[40]=((datahex&0xFF000000)>>24);
                        LabviewOut[41]=((datahex&0x00FF0000)>>16);
                        LabviewOut[42]=((datahex&0x0000FF00)>>8);
                        LabviewOut[43]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Target_distance;
                        LabviewOut[44]=((datahex&0xFF000000)>>24);
                        LabviewOut[45]=((datahex&0x00FF0000)>>16);
                        LabviewOut[46]=((datahex&0x0000FF00)>>8);
                        LabviewOut[47]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&GPS.Target_angle;
                        LabviewOut[48]=((datahex&0xFF000000)>>24);
                        LabviewOut[49]=((datahex&0x00FF0000)>>16);
                        LabviewOut[50]=((datahex&0x0000FF00)>>8);
                        LabviewOut[51]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&PIDDX;
                        LabviewOut[52]=((datahex&0xFF000000)>>24);
                        LabviewOut[53]=((datahex&0x00FF0000)>>16);
                        LabviewOut[54]=((datahex&0x0000FF00)>>8);
                        LabviewOut[55]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&PIDDY;
                        LabviewOut[56]=((datahex&0xFF000000)>>24);
                        LabviewOut[57]=((datahex&0x00FF0000)>>16);
                        LabviewOut[58]=((datahex&0x0000FF00)>>8);
                        LabviewOut[59]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=60;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 54 :
                    /* EasyCODE ( 0 
                    54:GPS PID X Navi */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //1
                        datahex=*(unsigned long*)&GPS.X_Error;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        //2
                        datahex=*(unsigned long*)&PvalueDX;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        //3
                        datahex=*(unsigned long*)&IvalueDX;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        //4
                        datahex=*(unsigned long*)&DFilterDX;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        //5
                        datahex=*(unsigned long*)&DvalueDX;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        //6
                        datafloat=GPS.Target_distance;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 55 :
                    /* EasyCODE ( 0 
                    55:GPS PID Y Navi */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //1
                        datahex=*(unsigned long*)&GPS.Y_Error;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        //2
                        datahex=*(unsigned long*)&PvalueDY;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        //3
                        datahex=*(unsigned long*)&IvalueDY;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        //4
                        datahex=*(unsigned long*)&DFilterDY;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        //5
                        datahex=*(unsigned long*)&DvalueDY;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        //6
                        datafloat=GPS.Target_distance;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 60 :
                    /* EasyCODE ( 0 
                    60:Acc.XYZ;AngleXYZ */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&Acc.X;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Acc.Y;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Acc.Z;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&AngleX;
                        LabviewOut[12]=((datahex&0xFF000000)>>24);
                        LabviewOut[13]=((datahex&0x00FF0000)>>16);
                        LabviewOut[14]=((datahex&0x0000FF00)>>8);
                        LabviewOut[15]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&AngleY;
                        LabviewOut[16]=((datahex&0xFF000000)>>24);
                        LabviewOut[17]=((datahex&0x00FF0000)>>16);
                        LabviewOut[18]=((datahex&0x0000FF00)>>8);
                        LabviewOut[19]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&AngleZ;
                        LabviewOut[20]=((datahex&0xFF000000)>>24);
                        LabviewOut[21]=((datahex&0x00FF0000)>>16);
                        LabviewOut[22]=((datahex&0x0000FF00)>>8);
                        LabviewOut[23]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=24;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 90 :
                    /* EasyCODE ( 0 
                    90:Tuning */
                    if ( TRANSMIT==0 )
                    {
                        if ( Finished==1 )
                        {
                            //Speed Settings
                            datafloat=Frequency;//-0.1;
                            datahex=*(unsigned long*)&datafloat;
                            LabviewOut[0]=((datahex&0xFF000000)>>24);
                            LabviewOut[1]=((datahex&0x00FF0000)>>16);
                            LabviewOut[2]=((datahex&0x0000FF00)>>8);
                            LabviewOut[3]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Re_cos_In_send;
                            LabviewOut[4]=((datahex&0xFF000000)>>24);
                            LabviewOut[5]=((datahex&0x00FF0000)>>16);
                            LabviewOut[6]=((datahex&0x0000FF00)>>8);
                            LabviewOut[7]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Im_sin_In_send;
                            LabviewOut[8]=((datahex&0xFF000000)>>24);
                            LabviewOut[9]=((datahex&0x00FF0000)>>16);
                            LabviewOut[10]=((datahex&0x0000FF00)>>8);
                            LabviewOut[11]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Re_cos_Out_send;
                            LabviewOut[12]=((datahex&0xFF000000)>>24);
                            LabviewOut[13]=((datahex&0x00FF0000)>>16);
                            LabviewOut[14]=((datahex&0x0000FF00)>>8);
                            LabviewOut[15]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Im_sin_Out_send;
                            LabviewOut[16]=((datahex&0xFF000000)>>24);
                            LabviewOut[17]=((datahex&0x00FF0000)>>16);
                            LabviewOut[18]=((datahex&0x0000FF00)>>8);
                            LabviewOut[19]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Re_cos_Signal_send;
                            LabviewOut[20]=((datahex&0xFF000000)>>24);
                            LabviewOut[21]=((datahex&0x00FF0000)>>16);
                            LabviewOut[22]=((datahex&0x0000FF00)>>8);
                            LabviewOut[23]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Im_sin_Signal_send;
                            LabviewOut[24]=((datahex&0xFF000000)>>24);
                            LabviewOut[25]=((datahex&0x00FF0000)>>16);
                            LabviewOut[26]=((datahex&0x0000FF00)>>8);
                            LabviewOut[27]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Re_cos_Error_send;
                            LabviewOut[28]=((datahex&0xFF000000)>>24);
                            LabviewOut[29]=((datahex&0x00FF0000)>>16);
                            LabviewOut[30]=((datahex&0x0000FF00)>>8);
                            LabviewOut[31]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Im_sin_Error_send;
                            LabviewOut[32]=((datahex&0xFF000000)>>24);
                            LabviewOut[33]=((datahex&0x00FF0000)>>16);
                            LabviewOut[34]=((datahex&0x0000FF00)>>8);
                            LabviewOut[35]= (datahex&0x000000FF);
                            /* EasyCODE - */
                            Finished=0;
                            /* EasyCODE - */
                            sendcount=0;
                            sendsize=36;
                            loopcounter=0;
                            TRANSMIT=1;
                        }
                    }
                    /* EasyCODE ) */
                    break;
                case 91 :
                    /* EasyCODE ( 0 
                    91:Injection */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        if ( Mode==2 )
                        {
                            //Angles
                            datahex=*(unsigned long*)&SignalA;
                            LabviewOut[0]=((datahex&0xFF000000)>>24);
                            LabviewOut[1]=((datahex&0x00FF0000)>>16);
                            LabviewOut[2]=((datahex&0x0000FF00)>>8);
                            LabviewOut[3]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&In;
                            LabviewOut[4]=((datahex&0xFF000000)>>24);
                            LabviewOut[5]=((datahex&0x00FF0000)>>16);
                            LabviewOut[6]=((datahex&0x0000FF00)>>8);
                            LabviewOut[7]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Out;
                            LabviewOut[8]=((datahex&0xFF000000)>>24);
                            LabviewOut[9]=((datahex&0x00FF0000)>>16);
                            LabviewOut[10]=((datahex&0x0000FF00)>>8);
                            LabviewOut[11]= (datahex&0x000000FF);
                        }
                        else
                        {
                            //Angles
                            datahex=*(unsigned long*)&SignalS;
                            LabviewOut[0]=((datahex&0xFF000000)>>24);
                            LabviewOut[1]=((datahex&0x00FF0000)>>16);
                            LabviewOut[2]=((datahex&0x0000FF00)>>8);
                            LabviewOut[3]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&In;
                            LabviewOut[4]=((datahex&0xFF000000)>>24);
                            LabviewOut[5]=((datahex&0x00FF0000)>>16);
                            LabviewOut[6]=((datahex&0x0000FF00)>>8);
                            LabviewOut[7]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Out;
                            LabviewOut[8]=((datahex&0xFF000000)>>24);
                            LabviewOut[9]=((datahex&0x00FF0000)>>16);
                            LabviewOut[10]=((datahex&0x0000FF00)>>8);
                            LabviewOut[11]= (datahex&0x000000FF);
                        }
                        sendcount=0;
                        sendsize=12;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 92 :
                    /* EasyCODE ( 0 
                    92:In-Signal */
                    if ( TRANSMIT==0 )
                    {
                        if ( loopcounter>((unsigned int)(0.025/dt)) )
                        {
                            //Angles
                            datahex=*(unsigned long*)&Re_cos_In;
                            LabviewOut[0]=((datahex&0xFF000000)>>24);
                            LabviewOut[1]=((datahex&0x00FF0000)>>16);
                            LabviewOut[2]=((datahex&0x0000FF00)>>8);
                            LabviewOut[3]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Im_sin_In;
                            LabviewOut[4]=((datahex&0xFF000000)>>24);
                            LabviewOut[5]=((datahex&0x00FF0000)>>16);
                            LabviewOut[6]=((datahex&0x0000FF00)>>8);
                            LabviewOut[7]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Frequency;
                            LabviewOut[8]=((datahex&0xFF000000)>>24);
                            LabviewOut[9]=((datahex&0x00FF0000)>>16);
                            LabviewOut[10]=((datahex&0x0000FF00)>>8);
                            LabviewOut[11]= (datahex&0x000000FF);
                            /* EasyCODE - */
                            sendcount=0;
                            sendsize=12;
                            loopcounter=0;
                            TRANSMIT=1;
                        }
                        else
                        {
                            loopcounter++;
                        }
                    }
                    /* EasyCODE ) */
                    break;
                case 93 :
                    /* EasyCODE ( 0 
                    93: Out-Signal */
                    if ( TRANSMIT==0 )
                    {
                        if ( loopcounter>((unsigned int)(0.025/dt)) )
                        {
                            //Angles
                            datahex=*(unsigned long*)&Re_cos_Out;
                            LabviewOut[0]=((datahex&0xFF000000)>>24);
                            LabviewOut[1]=((datahex&0x00FF0000)>>16);
                            LabviewOut[2]=((datahex&0x0000FF00)>>8);
                            LabviewOut[3]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Im_sin_Out;
                            LabviewOut[4]=((datahex&0xFF000000)>>24);
                            LabviewOut[5]=((datahex&0x00FF0000)>>16);
                            LabviewOut[6]=((datahex&0x0000FF00)>>8);
                            LabviewOut[7]= (datahex&0x000000FF);
                            
                            datahex=*(unsigned long*)&Frequency;
                            LabviewOut[8]=((datahex&0xFF000000)>>24);
                            LabviewOut[9]=((datahex&0x00FF0000)>>16);
                            LabviewOut[10]=((datahex&0x0000FF00)>>8);
                            LabviewOut[11]= (datahex&0x000000FF);
                            /* EasyCODE - */
                            sendcount=0;
                            sendsize=12;
                            loopcounter=0;
                            TRANSMIT=1;
                        }
                        else
                        {
                            loopcounter++;
                        }
                    }
                    /* EasyCODE ) */
                    break;
                case 95 :
                    /* EasyCODE ( 0 
                    95:Power */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Angles
                        datahex=*(unsigned long*)&Vlipo;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        
                        datahex=*(unsigned long*)&Current;
                        LabviewOut[4]=((datahex&0xFF000000)>>24);
                        LabviewOut[5]=((datahex&0x00FF0000)>>16);
                        LabviewOut[6]=((datahex&0x0000FF00)>>8);
                        LabviewOut[7]= (datahex&0x000000FF);
                        
                        datafloat=(Vlipo*Current);
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[8]=((datahex&0xFF000000)>>24);
                        LabviewOut[9]=((datahex&0x00FF0000)>>16);
                        LabviewOut[10]=((datahex&0x0000FF00)>>8);
                        LabviewOut[11]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=12;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 96 :
                    /* EasyCODE ( 0 
                    96: Temperature */
                    if ( TRANSMIT==0 )
                    {
                    }
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datahex=*(unsigned long*)&Gyro.TEMP;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=4;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 97 :
                    /* EasyCODE ( 0 
                    97:Error Code */
                    if ( TRANSMIT==0 )
                    {
                    }
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Battery
                        datafloat=E_code;
                        datahex=*(unsigned long*)&datafloat;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=4;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 98 :
                    /* EasyCODE ( 0 
                    98:CPUMax */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        datahex=*(unsigned long*)&TsloadMax;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=4;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                case 99 :
                    /* EasyCODE ( 0 
                    99:CPU */
                    if ( loopcounter>=((unsigned int)(Data_Interval/dt)) )
                    {
                        //Battery
                        datahex=*(unsigned long*)&Tsload;
                        LabviewOut[0]=((datahex&0xFF000000)>>24);
                        LabviewOut[1]=((datahex&0x00FF0000)>>16);
                        LabviewOut[2]=((datahex&0x0000FF00)>>8);
                        LabviewOut[3]= (datahex&0x000000FF);
                        /* EasyCODE - */
                        sendcount=0;
                        sendsize=4;
                        loopcounter=1;
                        TRANSMIT=1;
                    }
                    else
                    {
                        loopcounter++;
                    }
                    /* EasyCODE ) */
                    break;
                default:
                    break;
                }
                /* EasyCODE ) */
                /* EasyCODE ( 0 
                Send data */
                if ( TRANSMIT==1 )
                {
                    //BLED=1;
                    if ( sendcount!=0 )
                    {
                        if ( IFS1bits.U2TXIF==1 )
                        {
                            IFS1bits.U2TXIF=0;
                            if ( sendcount>=sendsize )
                            {
                                U2TXREG=0xED;
                                TRANSMIT=0;
                                sendcount=0;
                                sendsize=0;
                                /* EasyCODE - */
                                //BLED=0;
                            }
                            else
                            {
                                U2TXREG=((unsigned char)LabviewOut[sendcount]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+1]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+2]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+3]);
                                sendcount+=4;
                            }
                        }
                    }
                    else
                    {
                        U2TXREG=((unsigned char)LabviewOut[sendcount]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+1]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+2]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+3]);
                        sendcount+=4;
                    }
                }
                else
                {
                    IFS1bits.U2TXIF=0;
                }
                /* EasyCODE ) */
                if ( BT_RTS==0 )
                {
                }
                break;
            case 1 :
                /* EasyCODE ( 0 
                1: PID Attitude Settings */
                if ( Labview[48]=='*' && grote==48 )
                {
                    //Angle Settings
                    datahex=((Labview[0]<<24)+(Labview[1]<<16)+(Labview[2]<<8)+(Labview[3]));
                    KrXS=*(float*)&datahex;
                    datahex=((Labview[4]<<24)+(Labview[5]<<16)+(Labview[6]<<8)+(Labview[7]));
                    KiXS=*(float*)&datahex;
                    datahex=((Labview[8]<<24)+(Labview[9]<<16)+(Labview[10]<<8)+(Labview[11]));
                    KdXS=*(float*)&datahex;
                    
                    KrYS=KrXS;
                    KiYS=KiXS;
                    KdYS=KdXS;
                    
                    datahex=((Labview[12]<<24)+(Labview[13]<<16)+(Labview[14]<<8)+(Labview[15]));
                    KrZS=*(float*)&datahex;
                    datahex=((Labview[16]<<24)+(Labview[17]<<16)+(Labview[18]<<8)+(Labview[19]));
                    KiZS=*(float*)&datahex;
                    datahex=((Labview[20]<<24)+(Labview[21]<<16)+(Labview[22]<<8)+(Labview[23]));
                    KdZS=*(float*)&datahex;
                    /* EasyCODE - */
                    //Speed Settings
                    datahex=((Labview[24]<<24)+(Labview[25]<<16)+(Labview[26]<<8)+(Labview[27]));
                    KrX=*(float*)&datahex;
                    datahex=((Labview[28]<<24)+(Labview[29]<<16)+(Labview[30]<<8)+(Labview[31]));
                    KiX=*(float*)&datahex;
                    datahex=((Labview[32]<<24)+(Labview[33]<<16)+(Labview[34]<<8)+(Labview[35]));
                    KdX=*(float*)&datahex;
                    
                    KrY=KrX;
                    KiY=KiX;
                    KdY=KdX;
                    
                    datahex=((Labview[36]<<24)+(Labview[37]<<16)+(Labview[38]<<8)+(Labview[39]));
                    KrZ=*(float*)&datahex;
                    datahex=((Labview[40]<<24)+(Labview[41]<<16)+(Labview[42]<<8)+(Labview[43]));
                    KiZ=*(float*)&datahex;
                    datahex=((Labview[44]<<24)+(Labview[45]<<16)+(Labview[46]<<8)+(Labview[47]));
                    KdZ=*(float*)&datahex;
                    /* EasyCODE - */
                    Labview[48]=0;
                    grote=0;
                    /* EasyCODE ( 0 
                    Data Out */
                    //Angle Settings
                    datahex=*(unsigned long*)&KrXS;
                    LabviewOut[0]=((datahex&0xFF000000)>>24);
                    LabviewOut[1]=((datahex&0x00FF0000)>>16);
                    LabviewOut[2]=((datahex&0x0000FF00)>>8);
                    LabviewOut[3]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KiXS;
                    LabviewOut[4]=((datahex&0xFF000000)>>24);
                    LabviewOut[5]=((datahex&0x00FF0000)>>16);
                    LabviewOut[6]=((datahex&0x0000FF00)>>8);
                    LabviewOut[7]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KdXS;
                    LabviewOut[8]=((datahex&0xFF000000)>>24);
                    LabviewOut[9]=((datahex&0x00FF0000)>>16);
                    LabviewOut[10]=((datahex&0x0000FF00)>>8);
                    LabviewOut[11]= (datahex&0x000000FF);
                    
                    
                    datahex=*(unsigned long*)&KrZS;
                    LabviewOut[12]=((datahex&0xFF000000)>>24);
                    LabviewOut[13]=((datahex&0x00FF0000)>>16);
                    LabviewOut[14]=((datahex&0x0000FF00)>>8);
                    LabviewOut[15]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KiZS;
                    LabviewOut[16]=((datahex&0xFF000000)>>24);
                    LabviewOut[17]=((datahex&0x00FF0000)>>16);
                    LabviewOut[18]=((datahex&0x0000FF00)>>8);
                    LabviewOut[19]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KdZS;
                    LabviewOut[20]=((datahex&0xFF000000)>>24);
                    LabviewOut[21]=((datahex&0x00FF0000)>>16);
                    LabviewOut[22]=((datahex&0x0000FF00)>>8);
                    LabviewOut[23]= (datahex&0x000000FF);
                    /* EasyCODE - */
                    //Speed Settings
                    datahex=*(unsigned long*)&KrX;
                    LabviewOut[24]=((datahex&0xFF000000)>>24);
                    LabviewOut[25]=((datahex&0x00FF0000)>>16);
                    LabviewOut[26]=((datahex&0x0000FF00)>>8);
                    LabviewOut[27]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KiX;
                    LabviewOut[28]=((datahex&0xFF000000)>>24);
                    LabviewOut[29]=((datahex&0x00FF0000)>>16);
                    LabviewOut[30]=((datahex&0x0000FF00)>>8);
                    LabviewOut[31]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KdX;
                    LabviewOut[32]=((datahex&0xFF000000)>>24);
                    LabviewOut[33]=((datahex&0x00FF0000)>>16);
                    LabviewOut[34]=((datahex&0x0000FF00)>>8);
                    LabviewOut[35]= (datahex&0x000000FF);
                    
                    
                    datahex=*(unsigned long*)&KrZ;
                    LabviewOut[36]=((datahex&0xFF000000)>>24);
                    LabviewOut[37]=((datahex&0x00FF0000)>>16);
                    LabviewOut[38]=((datahex&0x0000FF00)>>8);
                    LabviewOut[39]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KiZ;
                    LabviewOut[40]=((datahex&0xFF000000)>>24);
                    LabviewOut[41]=((datahex&0x00FF0000)>>16);
                    LabviewOut[42]=((datahex&0x0000FF00)>>8);
                    LabviewOut[43]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KdZ;
                    LabviewOut[44]=((datahex&0xFF000000)>>24);
                    LabviewOut[45]=((datahex&0x00FF0000)>>16);
                    LabviewOut[46]=((datahex&0x0000FF00)>>8);
                    LabviewOut[47]= (datahex&0x000000FF);
                    /* EasyCODE ) */
                    sendcount=0;
                    sendsize=48;
                    loopcounter=0;
                    TRANSMIT=1;
                }
                if ( TRANSMIT==1 )
                {
                    if ( sendcount!=0 )
                    {
                        if ( IFS1bits.U2TXIF==1 )
                        {
                            IFS1bits.U2TXIF=0;
                            if ( sendcount>=sendsize )
                            {
                                U2TXREG=0xE5;
                                TRANSMIT=0;
                                sendcount=0;
                                sendsize=0;
                            }
                            else
                            {
                                U2TXREG=((unsigned char)LabviewOut[sendcount]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+1]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+2]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+3]);
                                sendcount+=4;
                            }
                        }
                    }
                    else
                    {
                        U2TXREG=((unsigned char)LabviewOut[sendcount]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+1]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+2]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+3]);
                        sendcount+=4;
                    }
                }
                else
                {
                    IFS1bits.U2TXIF=0;
                }
                /* EasyCODE ) */
                break;
            case 2 :
                /* EasyCODE ( 0 
                2: PID Altitude & Navisettings */
                if ( Labview[48]=='*' && grote==48 )
                {
                    //Altitude
                    datahex=((Labview[0]<<24)+(Labview[1]<<16)+(Labview[2]<<8)+(Labview[3]));
                    KrA=*(float*)&datahex;
                    datahex=((Labview[4]<<24)+(Labview[5]<<16)+(Labview[6]<<8)+(Labview[7]));
                    KiA=*(float*)&datahex;
                    datahex=((Labview[8]<<24)+(Labview[9]<<16)+(Labview[10]<<8)+(Labview[11]));
                    KdA=*(float*)&datahex;
                    
                    datahex=((Labview[12]<<24)+(Labview[13]<<16)+(Labview[14]<<8)+(Labview[15]));
                    KrAS=*(float*)&datahex;
                    datahex=((Labview[16]<<24)+(Labview[17]<<16)+(Labview[18]<<8)+(Labview[19]));
                    KiAS=*(float*)&datahex;
                    datahex=((Labview[20]<<24)+(Labview[21]<<16)+(Labview[22]<<8)+(Labview[23]));
                    KdAS=*(float*)&datahex;
                    /* EasyCODE - */
                    //Navigation
                    datahex=((Labview[24]<<24)+(Labview[25]<<16)+(Labview[26]<<8)+(Labview[27]));
                    KrN=*(float*)&datahex;
                    datahex=((Labview[28]<<24)+(Labview[29]<<16)+(Labview[30]<<8)+(Labview[31]));
                    KiN=*(float*)&datahex;
                    datahex=((Labview[32]<<24)+(Labview[33]<<16)+(Labview[34]<<8)+(Labview[35]));
                    KdN=*(float*)&datahex;
                    
                    datahex=((Labview[36]<<24)+(Labview[37]<<16)+(Labview[38]<<8)+(Labview[39]));
                    KrNS=*(float*)&datahex;
                    datahex=((Labview[40]<<24)+(Labview[41]<<16)+(Labview[42]<<8)+(Labview[43]));
                    KiNS=*(float*)&datahex;
                    datahex=((Labview[44]<<24)+(Labview[45]<<16)+(Labview[46]<<8)+(Labview[47]));
                    KdNS=*(float*)&datahex;
                    /* EasyCODE - */
                    Labview[48]=0;
                    grote=0;
                    /* EasyCODE ( 0 
                    Data Out */
                    //Altitude Settings
                    datahex=*(unsigned long*)&KrA;
                    LabviewOut[0]=((datahex&0xFF000000)>>24);
                    LabviewOut[1]=((datahex&0x00FF0000)>>16);
                    LabviewOut[2]=((datahex&0x0000FF00)>>8);
                    LabviewOut[3]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KiA;
                    LabviewOut[4]=((datahex&0xFF000000)>>24);
                    LabviewOut[5]=((datahex&0x00FF0000)>>16);
                    LabviewOut[6]=((datahex&0x0000FF00)>>8);
                    LabviewOut[7]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KdA;
                    LabviewOut[8]=((datahex&0xFF000000)>>24);
                    LabviewOut[9]=((datahex&0x00FF0000)>>16);
                    LabviewOut[10]=((datahex&0x0000FF00)>>8);
                    LabviewOut[11]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KrAS;
                    LabviewOut[12]=((datahex&0xFF000000)>>24);
                    LabviewOut[13]=((datahex&0x00FF0000)>>16);
                    LabviewOut[14]=((datahex&0x0000FF00)>>8);
                    LabviewOut[15]= (datahex&0x000000FF);
                    
                    
                    datahex=*(unsigned long*)&KiAS;
                    LabviewOut[16]=((datahex&0xFF000000)>>24);
                    LabviewOut[17]=((datahex&0x00FF0000)>>16);
                    LabviewOut[18]=((datahex&0x0000FF00)>>8);
                    LabviewOut[19]= (datahex&0x000000FF);
                    
                    
                    datahex=*(unsigned long*)&KdAS;
                    LabviewOut[20]=((datahex&0xFF000000)>>24);
                    LabviewOut[21]=((datahex&0x00FF0000)>>16);
                    LabviewOut[22]=((datahex&0x0000FF00)>>8);
                    LabviewOut[23]= (datahex&0x000000FF);
                    
                    //Postion
                    
                    datahex=*(unsigned long*)&KrN;
                    LabviewOut[24]=((datahex&0xFF000000)>>24);
                    LabviewOut[25]=((datahex&0x00FF0000)>>16);
                    LabviewOut[26]=((datahex&0x0000FF00)>>8);
                    LabviewOut[27]= (datahex&0x000000FF);
                    
                    
                    datahex=*(unsigned long*)&KiN;
                    LabviewOut[28]=((datahex&0xFF000000)>>24);
                    LabviewOut[29]=((datahex&0x00FF0000)>>16);
                    LabviewOut[30]=((datahex&0x0000FF00)>>8);
                    LabviewOut[31]= (datahex&0x000000FF);
                    
                    
                    datahex=*(unsigned long*)&KdN;
                    LabviewOut[32]=((datahex&0xFF000000)>>24);
                    LabviewOut[33]=((datahex&0x00FF0000)>>16);
                    LabviewOut[34]=((datahex&0x0000FF00)>>8);
                    LabviewOut[35]= (datahex&0x000000FF);
                    
                    
                    datahex=*(unsigned long*)&KrNS;
                    LabviewOut[36]=((datahex&0xFF000000)>>24);
                    LabviewOut[37]=((datahex&0x00FF0000)>>16);
                    LabviewOut[38]=((datahex&0x0000FF00)>>8);
                    LabviewOut[39]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KiNS;
                    LabviewOut[40]=((datahex&0xFF000000)>>24);
                    LabviewOut[41]=((datahex&0x00FF0000)>>16);
                    LabviewOut[42]=((datahex&0x0000FF00)>>8);
                    LabviewOut[43]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&KdNS;
                    LabviewOut[44]=((datahex&0xFF000000)>>24);
                    LabviewOut[45]=((datahex&0x00FF0000)>>16);
                    LabviewOut[46]=((datahex&0x0000FF00)>>8);
                    LabviewOut[47]= (datahex&0x000000FF);
                    /* EasyCODE ) */
                    sendcount=0;
                    sendsize=48;
                    loopcounter=0;
                    TRANSMIT=1;
                }
                if ( TRANSMIT==1 )
                {
                    if ( sendcount!=0 )
                    {
                        if ( IFS1bits.U2TXIF==1 )
                        {
                            IFS1bits.U2TXIF=0;
                            if ( sendcount>=sendsize )
                            {
                                U2TXREG=0xE5;
                                TRANSMIT=0;
                                sendcount=0;
                                sendsize=0;
                            }
                            else
                            {
                                U2TXREG=((unsigned char)LabviewOut[sendcount]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+1]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+2]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+3]);
                                sendcount+=4;
                            }
                        }
                    }
                    else
                    {
                        U2TXREG=((unsigned char)LabviewOut[sendcount]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+1]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+2]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+3]);
                        sendcount+=4;
                    }
                }
                else
                {
                    IFS1bits.U2TXIF=0;
                }
                /* EasyCODE ) */
                break;
            case 3 :
                /* EasyCODE ( 0 
                3: Filter settings */
                if ( Labview[28]=='*' && grote==28 )
                {
                    datahex=((Labview[0]<<24)+(Labview[1]<<16)+(Labview[2]<<8)+(Labview[3]));
                    betasetting=*(float*)&datahex;
                    datahex=((Labview[4]<<24)+(Labview[5]<<16)+(Labview[6]<<8)+(Labview[7]));
                    AngleLimit=*(float*)&datahex;
                    
                    datahex=((Labview[8]<<24)+(Labview[9]<<16)+(Labview[10]<<8)+(Labview[11]));
                    SpeedGain=*(float*)&datahex;
                    datahex=((Labview[12]<<24)+(Labview[13]<<16)+(Labview[14]<<8)+(Labview[15]));
                    AltitudeGain=*(float*)&datahex;
                    datahex=((Labview[16]<<24)+(Labview[17]<<16)+(Labview[18]<<8)+(Labview[19]));
                    LPFGain=*(float*)&datahex;
                    
                    datahex=((Labview[20]<<24)+(Labview[21]<<16)+(Labview[22]<<8)+(Labview[23]));
                    LPFGainA=*(float*)&datahex;
                    
                    datahex=((Labview[24]<<24)+(Labview[25]<<16)+(Labview[26]<<8)+(Labview[27]));
                    LPFGainD=*(float*)&datahex;
                    /* EasyCODE - */
                    Labview[28]=0;
                    grote=0;
                    /* EasyCODE ( 0 
                    Data Out */
                    //Angle Settings
                    datahex=*(unsigned long*)&betasetting;
                    LabviewOut[0]=((datahex&0xFF000000)>>24);
                    LabviewOut[1]=((datahex&0x00FF0000)>>16);
                    LabviewOut[2]=((datahex&0x0000FF00)>>8);
                    LabviewOut[3]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&AngleLimit;
                    LabviewOut[4]=((datahex&0xFF000000)>>24);
                    LabviewOut[5]=((datahex&0x00FF0000)>>16);
                    LabviewOut[6]=((datahex&0x0000FF00)>>8);
                    LabviewOut[7]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&SpeedGain;
                    LabviewOut[8]=((datahex&0xFF000000)>>24);
                    LabviewOut[9]=((datahex&0x00FF0000)>>16);
                    LabviewOut[10]=((datahex&0x0000FF00)>>8);
                    LabviewOut[11]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&AltitudeGain;
                    LabviewOut[12]=((datahex&0xFF000000)>>24);
                    LabviewOut[13]=((datahex&0x00FF0000)>>16);
                    LabviewOut[14]=((datahex&0x0000FF00)>>8);
                    LabviewOut[15]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&LPFGain;
                    LabviewOut[16]=((datahex&0xFF000000)>>24);
                    LabviewOut[17]=((datahex&0x00FF0000)>>16);
                    LabviewOut[18]=((datahex&0x0000FF00)>>8);
                    LabviewOut[19]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&LPFGainA;
                    LabviewOut[20]=((datahex&0xFF000000)>>24);
                    LabviewOut[21]=((datahex&0x00FF0000)>>16);
                    LabviewOut[22]=((datahex&0x0000FF00)>>8);
                    LabviewOut[23]= (datahex&0x000000FF);
                    
                    datahex=*(unsigned long*)&LPFGainD;
                    LabviewOut[24]=((datahex&0xFF000000)>>24);
                    LabviewOut[25]=((datahex&0x00FF0000)>>16);
                    LabviewOut[26]=((datahex&0x0000FF00)>>8);
                    LabviewOut[27]= (datahex&0x000000FF);
                    /* EasyCODE ) */
                    sendcount=0;
                    sendsize=28;
                    loopcounter=0;
                    TRANSMIT=1;
                }
                if ( TRANSMIT==1 )
                {
                    if ( sendcount!=0 )
                    {
                        if ( IFS1bits.U2TXIF==1 )
                        {
                            IFS1bits.U2TXIF=0;
                            if ( sendcount>=sendsize )
                            {
                                U2TXREG=0xE5;
                                TRANSMIT=0;
                                sendcount=0;
                                sendsize=0;
                            }
                            else
                            {
                                U2TXREG=((unsigned char)LabviewOut[sendcount]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+1]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+2]);
                                U2TXREG=((unsigned char)LabviewOut[sendcount+3]);
                                sendcount+=4;
                            }
                        }
                    }
                    else
                    {
                        U2TXREG=((unsigned char)LabviewOut[sendcount]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+1]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+2]);
                        U2TXREG=((unsigned char)LabviewOut[sendcount+3]);
                        sendcount+=4;
                    }
                }
                else
                {
                    IFS1bits.U2TXIF=0;
                }
                /* EasyCODE ) */
                break;
            default:
                break;
            }
            /* EasyCODE ( 0 
            UART Error */
            if ( U2STAbits.OERR )
            {
                U2STAbits.OERR=0;
            }
            if ( U2STAbits.OERR )
            {
                U2STAbits.OERR=0;
            }
            if ( U6STAbits.OERR )
            {
                U6STAbits.OERR=0;
            }
            /* EasyCODE ) */
            if ( SendSensor==90 && Throttle!=0 )
            {
                FreqResponse=1;
            }
            else
            {
                FreqResponse=0;
            }
            /* EasyCODE ) */
            /* EasyCODE ( 0 
            DSMX Control */
            if ( UART2start==1 )
            {
                for ( DSMXcounter=0; DSMXcounter<14;DSMXcounter+=2 )
                {
                    DSMXdata=((DSMX[DSMXcounter]<<8)+DSMX[DSMXcounter+1]);
                    Channel=((DSMXdata&0x7800)>>0x0B);
                    DSMXdata=(DSMXdata&0x07FF);
                    DSMX[DSMXcounter]=0;
                    DSMX[DSMXcounter+1]=0;
                    /* EasyCODE ( 0 
                    Channel Data */
                    switch ( Channel )
                    {
                    case 0 :
                        DSMX_Data.CH0=DSMXdata;
                        break;
                    case 1 :
                        DSMX_Data.CH1=DSMXdata;
                        break;
                    case 2 :
                        DSMX_Data.CH2=DSMXdata;
                        break;
                    case 3 :
                        DSMX_Data.CH3=DSMXdata;
                        break;
                    case 4 :
                        DSMX_Data.CH4=DSMXdata;
                        break;
                    case 5 :
                        DSMX_Data.CH5=DSMXdata;
                        break;
                    case 6 :
                        DSMX_Data.CH6=DSMXdata;
                        break;
                    default:
                        break;
                    }
                    /* EasyCODE ) */
                }
                UART2start=0;
                /* EasyCODE ( 0 
                Mode */
                if ( DSMX_Data.CH4==1918 )
                {
                    Mode=1; //Acro Mode
                }
                else
                {
                    if ( DSMX_Data.CH4==126 )
                    {
                        Mode=2; //Stable Mode
                    }
                    else
                    {
                        Mode=0;
                    }
                }
                /* EasyCODE ) */
                /* EasyCODE ( 0 
                Throttle */
                if ( DSMX_Data.CH0==0 )
                {
                    if ( DSMX_Throttle==0 )
                    {
                        if ( Throttle>1 )
                        {
                            Throttle=0;
                            ThrottleStick=0;
                            SetpointZ=0;
                            SetpointX=0;
                            SetpointY=0;
                        }
                        else
                        {
                            Throttle=1000;
                        }
                    }
                    AltitudeHoldOn=0;
                    AltitudeControlOn=0;
                    /* EasyCODE - */
                    DSMX_Throttle=1;
                }
                else
                {
                    DSMX_Throttle=0;
                }
                /* EasyCODE ) */
                /* EasyCODE_STRUCTURED_COMMENT_START */
                /*
                EasyCODE_COMMENT_START EasyCODE ( 0 
                Throttle Stick EasyCODE_COMMENT_END
                if ( DSMX_Data.CH0==1022 )
                {
                    if ( AltitudeHoldOn==1 )
                    {
                    }
                    else
                    {
                        SetpointAFilter=Altitude;
                        GPS.SetLat=GPS.HomeLat;
                        GPS.SetLong=GPS.HomeLong;
                    }
                    AltitudeHoldOn=1;
                    AltitudeControlOn=0;
                }
                else
                {
                    if ( DSMX_Data.CH0==1470 )
                    {
                        if ( AltitudeControlOn==1 )
                        {
                        }
                        else
                        {
                            SetpointAFilter=Altitude;
                            GPS.SetLat=GPS.Latitude;
                            GPS.SetLong=GPS.Longitude;
                        }
                        AltitudeHoldOn=0;
                        AltitudeControlOn=1;
                    }
                    else
                    {
                        if ( DSMX_Data.CH0==1918 )
                        {
                            AltitudeHoldOn=0;
                            AltitudeControlOn=0;
                            EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                            GPS.SetLat=GPS.Latitude;
                            GPS.SetLong=GPS.Longitude;
                        }
                        else
                        {
                            AltitudeHoldOn=0;
                            AltitudeControlOn=0;
                        }
                    }
                }
                if ( AltitudeHoldOn==1 )
                {
                    SetpointAspeed=DSMX_Data.CH5;
                    SetpointAspeed-=128;
                    SetpointAspeed-=895;
                    //SetpointAspeed*=0.003352;//3/(1790/2)=3.352m
                    SetpointAspeed*=0.004469;//4/(1790/2)=4.469m
                    if ( SetpointAspeed<0.2 &&SetpointAspeed>-0.2 )
                    {
                        SetpointAspeed=0;
                    }
                    if ( Altitude<2 && SetpointAspeed<-0.6 )
                    {
                        SetpointAspeed=-0.6;
                    }
                    else
                    {
                        if ( Altitude<6 && SetpointAspeed<-1 )
                        {
                            SetpointAspeed=-1;
                        }
                    }
                }
                if ( AltitudeControlOn==1 || AltitudeHoldOn==1 )
                {
                    switch ( Max_Altitude )
                    {
                    case 5 :
                        EasyCODE_COMMENT_START EasyCODE ( 0 
                        5M EasyCODE_COMMENT_END
                        SetpointA=DSMX_Data.CH5;
                        SetpointA-=128;
                        SetpointA*=0.002793; //5/1790=0.002793
                        SetpointA+=1.5;
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                        if ( SetpointAFilter<1 )
                        {
                            SetpointAFilter=1;
                        }
                        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                        break;
                    case 10 :
                        EasyCODE_COMMENT_START EasyCODE ( 0 
                        10M EasyCODE_COMMENT_END
                        SetpointA=DSMX_Data.CH5;
                        SetpointA-=128;
                        SetpointA*=0.005587; //10/1790=0.005587
                        SetpointA+=1.5;
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                        if ( SetpointAFilter<1 )
                        {
                            SetpointAFilter=1;
                        }
                        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                        break;
                    case 20 :
                        EasyCODE_COMMENT_START EasyCODE ( 0 
                        20M EasyCODE_COMMENT_END
                        SetpointA=DSMX_Data.CH5;
                        SetpointA-=128;
                        SetpointA*=0.01117; //20/1790=0.01117
                        SetpointA+=1.5;
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                        if ( SetpointAFilter<1 )
                        {
                            SetpointAFilter=1;
                        }
                        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                        break;
                    case 30 :
                        EasyCODE_COMMENT_START EasyCODE ( 0 
                        30M EasyCODE_COMMENT_END
                        SetpointA=DSMX_Data.CH5;
                        SetpointA-=128;
                        SetpointA*=0.01676; //30/1790=0.01676
                        SetpointA+=1.5;
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                        if ( SetpointAFilter<1 )
                        {
                            SetpointAFilter=1;
                        }
                        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                        break;
                    case 100 :
                        EasyCODE_COMMENT_START EasyCODE ( 0 
                        100M EasyCODE_COMMENT_END
                        SetpointA=DSMX_Data.CH5;
                        SetpointA-=128;
                        SetpointA*=0.05587; //100/1790=0.05587
                        SetpointA+=1.5;
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                        if ( SetpointAFilter<1 )
                        {
                            SetpointAFilter=1;
                        }
                        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                        break;
                    default:
                        EasyCODE_COMMENT_START EasyCODE ( 0 
                        2M EasyCODE_COMMENT_END
                        SetpointA=DSMX_Data.CH5;
                        SetpointA-=128;
                        SetpointA*=0.001117; //2/1790=0.001117
                        SetpointA+=1.5;
                        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
                        SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                        if ( SetpointAFilter<1 )
                        {
                            SetpointAFilter=1;
                        }
                        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                        break;
                    }
                }
                else
                {
                    ThrottleStick=DSMX_Data.CH5;
                    ThrottleStick-=128;
                    ThrottleStick*=3.295;//5900/1790=3.296
                    if ( Throttle==0 )
                    {
                        ThrottleStick=0;
                    }
                }
                if ( Throttle==0 )
                {
                    GPS.HomeLat=GPS.Latitude;
                    GPS.HomeLong=GPS.Longitude;
                    GPS.SetLat=GPS.Latitude;
                    GPS.SetLong=GPS.Longitude;
                }
                EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                */
                /* EasyCODE_STRUCTURED_COMMENT_END */
                /* EasyCODE ( 0 
                Throttle Stick */
                if ( DSMX_Data.CH0==1022 )
                {
                    if ( AltitudeHoldOn==1 )
                    {
                    }
                    else
                    {
                        GPS.SetLat=GPS.HomeLat;
                        GPS.SetLong=GPS.HomeLong;
                    }
                    AltitudeHoldOn=1;
                    AltitudeControlOn=0;
                }
                else
                {
                    if ( DSMX_Data.CH0==1470 )
                    {
                        if ( AltitudeControlOn==1 )
                        {
                        }
                        else
                        {
                            SetpointAFilter=Altitude;
                            GPS.SetLat=GPS.Latitude;
                            GPS.SetLong=GPS.Longitude;
                        }
                        AltitudeHoldOn=0;
                        AltitudeControlOn=1;
                    }
                    else
                    {
                        if ( DSMX_Data.CH0==1918 )
                        {
                            //Manual Mode
                        }
                        GPS.SetLat=GPS.Latitude;
                        GPS.SetLong=GPS.Longitude;
                        /* EasyCODE - */
                        AltitudeHoldOn=0;
                        AltitudeControlOn=0;
                        if ( Throttle==0 )
                        {
                            GPS.HomeLat=GPS.Latitude;
                            GPS.HomeLong=GPS.Longitude;
                        }
                    }
                }
                if ( AltitudeHoldOn==1 )
                {
                    SetpointAspeed=DSMX_Data.CH5;
                    SetpointAspeed-=128;
                    SetpointAspeed-=895;
                    //SetpointAspeed*=0.003352;//3/(1790/2)=3.352m
                    SetpointAspeed*=0.004469;//4/(1790/2)=4.469m
                    if ( SetpointAspeed<0.2 &&SetpointAspeed>-0.2 )
                    {
                        SetpointAspeed=0;
                    }
                    if ( Altitude<2 && SetpointAspeed<-0.6 )
                    {
                        SetpointAspeed=-0.6;
                    }
                    else
                    {
                        if ( Altitude<6 && SetpointAspeed<-1 )
                        {
                            SetpointAspeed=-1;
                        }
                    }
                }
                else
                {
                    if ( AltitudeControlOn==1 )
                    {
                        switch ( Max_Altitude )
                        {
                        case 5 :
                            /* EasyCODE ( 0 
                            5M */
                            SetpointA=DSMX_Data.CH5;
                            SetpointA-=128;
                            SetpointA*=0.002793; //5/1790=0.002793
                            SetpointA+=1.5;
                            /* EasyCODE - */
                            SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                            if ( SetpointAFilter<1 )
                            {
                                SetpointAFilter=1;
                            }
                            /* EasyCODE ) */
                            break;
                        case 10 :
                            /* EasyCODE ( 0 
                            10M */
                            SetpointA=DSMX_Data.CH5;
                            SetpointA-=128;
                            SetpointA*=0.005587; //10/1790=0.005587
                            SetpointA+=1.5;
                            /* EasyCODE - */
                            SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                            if ( SetpointAFilter<1 )
                            {
                                SetpointAFilter=1;
                            }
                            /* EasyCODE ) */
                            break;
                        case 20 :
                            /* EasyCODE ( 0 
                            20M */
                            SetpointA=DSMX_Data.CH5;
                            SetpointA-=128;
                            SetpointA*=0.01117; //20/1790=0.01117
                            SetpointA+=1.5;
                            /* EasyCODE - */
                            SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                            if ( SetpointAFilter<1 )
                            {
                                SetpointAFilter=1;
                            }
                            /* EasyCODE ) */
                            break;
                        case 30 :
                            /* EasyCODE ( 0 
                            30M */
                            SetpointA=DSMX_Data.CH5;
                            SetpointA-=128;
                            SetpointA*=0.01676; //30/1790=0.01676
                            SetpointA+=1.5;
                            /* EasyCODE - */
                            SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                            if ( SetpointAFilter<1 )
                            {
                                SetpointAFilter=1;
                            }
                            /* EasyCODE ) */
                            break;
                        case 100 :
                            /* EasyCODE ( 0 
                            100M */
                            SetpointA=DSMX_Data.CH5;
                            SetpointA-=128;
                            SetpointA*=0.05587; //100/1790=0.05587
                            SetpointA+=1.5;
                            /* EasyCODE - */
                            SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                            if ( SetpointAFilter<1 )
                            {
                                SetpointAFilter=1;
                            }
                            /* EasyCODE ) */
                            break;
                        default:
                            /* EasyCODE ( 0 
                            2M */
                            SetpointA=DSMX_Data.CH5;
                            SetpointA-=128;
                            SetpointA*=0.001117; //2/1790=0.001117
                            SetpointA+=1.5;
                            /* EasyCODE - */
                            SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                            if ( SetpointAFilter<1 )
                            {
                                SetpointAFilter=1;
                            }
                            /* EasyCODE ) */
                            break;
                        }
                    }
                    else
                    {
                        ThrottleStick=DSMX_Data.CH5;
                        ThrottleStick-=128;
                        ThrottleStick*=3.5;//6265/1790=3.5
                        if ( Throttle==0 )
                        {
                            ThrottleStick=0;
                        }
                    }
                }
                /* EasyCODE ) */
                if ( Mode==1 )
                {
                    /* EasyCODE ( 0 
                    X  - Velocity */
                    SetpointXspeed=DSMX_Data.CH2;
                    SetpointXspeed-=1024;
                    SetpointXspeed*=0.4474;  //400/(1918-1024)
                    SetpointXspeed*=-1;
                    /* EasyCODE ) */
                    /* EasyCODE ( 0 
                    Y  - Velocity */
                    SetpointYspeed=DSMX_Data.CH3;
                    SetpointYspeed-=1024;
                    SetpointYspeed*=0.4474;  //400/(1918-1024)
                    /* EasyCODE ) */
                    /* EasyCODE ( 0 
                    Z - Velocity */
                    SetpointZspeed=DSMX_Data.CH1;
                    SetpointZspeed-=1022;
                    //SetpointZspeed*=0.1112;  //100/(1918-1022)
                    SetpointZspeed*=0.1674;  //150/(1918-1022)
                    /* EasyCODE ) */
                }
                else
                {
                    if ( Mode==2 )
                    {
                        /* EasyCODE ( 0 
                        X - Angle */
                        SetpointX=DSMX_Data.CH2;
                        SetpointX-=1024;
                        SetpointX*=0.05593;  //50/(1918-1024)
                        SetpointX*=-1;
                        /* EasyCODE ) */
                        /* EasyCODE ( 0 
                        Y - Angle */
                        SetpointY=DSMX_Data.CH3;
                        SetpointY-=1024;
                        SetpointY*=0.05593;  //50/(1918-1024)
                        /* EasyCODE ) */
                        /* EasyCODE_STRUCTURED_COMMENT_START */
                        /*
                        EasyCODE_COMMENT_START EasyCODE ( 0 
                        Z - Angle EasyCODE_COMMENT_END
                        DSMXdatafloat=DSMX_Data.CH1;
                        DSMXdatafloat-=1022;
                        DSMXdatafloat*=0.002790;  //2.5/(1918-1022)
                        SetpointZ+=DSMXdatafloat;
                        if ( SetpointZ>360 )
                        {
                            SetpointZ=0;
                        }
                        else
                        {
                            if ( SetpointZ<0 )
                            {
                                SetpointZ=360;
                            }
                        }
                        if ( Throttle==0 )
                        {
                            SetpointZ=0;
                        }
                        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                        */
                        /* EasyCODE_STRUCTURED_COMMENT_END */
                        /* EasyCODE ( 0 
                        Z - Velocity */
                        SetpointZspeed=DSMX_Data.CH1;
                        SetpointZspeed-=1022;
                        //SetpointZspeed*=0.1112;  //100/(1918-1022)
                        SetpointZspeed*=0.1674;  //150/(1918-1022)
                        /* EasyCODE ) */
                    }
                    else
                    {
                        SetpointX=0;
                        SetpointY=0;
                        SetpointZ=0;
                    }
                }
            }
            else
            {
                GLED=0;
            }
            /* EasyCODE ) */
            /* EasyCODE ( 0 
            AHRS Setting */
            /* EasyCODE_STRUCTURED_COMMENT_START */
            /*
            if ( Throttle==0 )
            {
                beta=betastart;
            }
            else
            {
                beta=betasetting;
            }
            */
            /* EasyCODE_STRUCTURED_COMMENT_END */
            if ( beta<=betasetting )
            {
                beta=betasetting;
            }
            else
            {
                beta-=0.0005;
            }
            /* EasyCODE ) */
            /* EasyCODE ( 0 
            Remote Signal Check */
            if ( RemoteCounter>800 )
            {
                //OLED=0;
                GLED=0;
                SetpointX=0;
                SetpointY=0;
                SetpointZ=0;
                SetpointZspeed=0;
                //SetpointAspeed=-0.6;
                AltitudeHoldOn=0;
                AltitudeControlOn=1;
                //SetpointA=1;
                if ( E_RF==0 )
                {
                    SetpointAFilter=Altitude;
                    SetpointA=30;
                    GPS.SetLat=GPS.HomeLat;
                    GPS.SetLong=GPS.HomeLong;
                }
                E_RF=1;
                /* EasyCODE - */
                SetpointAFilter=SetpointAFilter+(0.01*(SetpointA-SetpointAFilter));
                if ( ThrottleStick>4700 )
                {
                    ThrottleStick=4700;
                }
            }
            else
            {
                RemoteCounter++;
                E_RF=0;
            }
            /* EasyCODE ) */
            /* EasyCODE ( 0 
            Battery */
            Vlipo=ADC1BUF0;
            Vlipo*=0.0637441406;
            VLIPO=Vlipo*100;
            if ( N_Cells==4 )
            {
                /* EasyCODE ( 0 
                4 Cell */
                if ( Vlipo<=12.5f )
                {
                    E_bat_empty=1;
                    /* EasyCODE - */
                    RLED=1;
                }
                else
                {
                    if ( Vlipo<13.0f )
                    {
                        E_bat_war=1;
                        /* EasyCODE - */
                        RLED=1;
                    }
                    else
                    {
                        if ( Vlipo>13.2f )
                        {
                            E_bat_war=0;
                            E_bat_empty=0;
                            /* EasyCODE - */
                            RLED=0;
                        }
                        else
                        {
                            RLED=0;
                        }
                    }
                }
                /* EasyCODE ) */
            }
            else
            {
                if ( N_Cells==3 )
                {
                    /* EasyCODE ( 0 
                    3 Cell */
                    if ( Vlipo<=9.2f )
                    {
                        E_bat_empty=1;
                        /* EasyCODE - */
                        RLED=1;
                    }
                    else
                    {
                        if ( Vlipo<9.4f )
                        {
                            E_bat_war=1;
                            /* EasyCODE - */
                            RLED=1;
                        }
                        else
                        {
                            if ( Vlipo>10.0f )
                            {
                                E_bat_war=0;
                                E_bat_empty=0;
                                /* EasyCODE - */
                                RLED=0;
                            }
                            else
                            {
                                RLED=0;
                            }
                        }
                    }
                    /* EasyCODE ) */
                }
            }
            /* EasyCODE ) */
            /* EasyCODE ( 0 
            Current */
            Current=ADC1BUF1;
            Current*=0.003193359;
            Current-=1.56;
            Current/=0.01175;
            /* EasyCODE ) */
            /* EasyCODE ( 0 
            GPS */
            if ( GPS.GPRMC[18]=='A' )
            {
                //Valid
                GPS.Status=1;
                RLED=0;
                if ( GPS.GPGGA[0]=='$' )
                {
                    GPS.DegLat=((GPS.GPGGA[18]-48)*10);
                    GPS.DegLat+=(GPS.GPGGA[19]-48);
                    /* EasyCODE - */
                    GPS.TimeLat=((GPS.GPGGA[20]-48)*10);
                    GPS.TimeLat+=((GPS.GPGGA[21]-48));
                    GPS.TimeLat*=10;
                    GPS.TimeLat+=((GPS.GPGGA[23]-48));
                    GPS.TimeLat*=10;
                    GPS.TimeLat+=((GPS.GPGGA[24]-48));
                    GPS.TimeLat*=10;
                    GPS.TimeLat+=((GPS.GPGGA[25]-48));
                    GPS.TimeLat*=10;
                    GPS.TimeLat+=((GPS.GPGGA[26]-48));
                    /* EasyCODE - */
                    GPS.DegLon=((GPS.GPGGA[31]-48)*10);
                    GPS.DegLon+=(GPS.GPGGA[32]-48);
                    /* EasyCODE - */
                    GPS.TimeLon=((GPS.GPGGA[33]-48)*10);
                    GPS.TimeLon+=((GPS.GPGGA[34]-48));
                    GPS.TimeLon*=10;
                    GPS.TimeLon+=((GPS.GPGGA[36]-48));
                    GPS.TimeLon*=10;
                    GPS.TimeLon+=((GPS.GPGGA[37]-48));
                    GPS.TimeLon*=10;
                    GPS.TimeLon+=((GPS.GPGGA[38]-48));
                    GPS.TimeLon*=10;
                    GPS.TimeLon+=((GPS.GPGGA[39]-48));
                    /* EasyCODE - */
                    GPS.Latitude=(GPS.DegLat+((float)GPS.TimeLat/600000));
                    GPS.Longitude=(GPS.DegLon+((float)GPS.TimeLon/600000));
                    /* EasyCODE - */
                    GPS.Altitude=(GPS.GPGGA[52]-48);
                    GPS.Altitude*=10;
                    GPS.Altitude+=(GPS.GPGGA[53]-48);
                    GPS.Altitude*=10;
                    GPS.Altitude+=(GPS.GPGGA[55]-48);
                    GPS.Altitude/=10;
                    if ( GPS.GPGGA[46]!=',' )
                    {
                        GPS.N_Sat=(GPS.GPGGA[45]-48);
                        GPS.N_Sat*=10;
                        GPS.N_Sat+=(GPS.GPGGA[46]-48);
                    }
                    else
                    {
                        GPS.N_Sat=(GPS.GPGGA[45]-48);
                    }
                    GPS.GPGGA[0]=0;
                    /* EasyCODE - */
                    OLED^=1;
                }
                if ( GPS.GPRMC[0]=='$' )
                {
                    int pos = 45;
                    pos = gpsReadDecimal(&GPS.Speed, pos);
                    pos = gpsReadDecimal(&GPS.Heading, pos+1);
                    GPS.GPRMC[0]=0;
                }
            }
            else
            {
                if ( GPS.GPRMC[18]=='V' )
                {
                    //Invalid
                    RLED=1;
                    GPS.Status=0;
                }
            }
            /* EasyCODE ) */
            if ( GPS.GPRMC[18]=='A' )
            {
                target();
            }
            /* EasyCODE ( 0 
            CPU Load */
            if ( TsloadMax>=(dt-0.0002) )
            {
                E_CPU=1;
            }
            else
            {
                //E_CPU=0;
            }
            /* EasyCODE ) */
            /* EasyCODE ( 0 
            Error */
            if ( E_bat_empty==1 )
            {
                /* EasyCODE ( 0 
                Buzzer 100/50 */
                if ( E_count>100 )
                {
                    BUZZER=0;
                    BACKLED=0;
                    BACKLED2=0;
                    SIDELED=0;
                    E_count=0;
                }
                else
                {
                    if ( E_count>50 )
                    {
                        BUZZER=0;
                        BACKLED=0;
                        BACKLED2=0;
                        SIDELED=0;
                        E_count++;
                    }
                    else
                    {
                        BUZZER=1;
                        BACKLED=1;
                        BACKLED2=1;
                        SIDELED=1;
                        E_count++;
                    }
                }
                /* EasyCODE ) */
                E_code=4;
            }
            else
            {
                if ( E_RF==1 )
                {
                    /* EasyCODE ( 0 
                    Buzzer 200/50 */
                    if ( E_count>200 )
                    {
                        BUZZER=0;
                        BACKLED=0;
                        BACKLED2=0;
                        E_count=0;
                    }
                    else
                    {
                        if ( E_count>50 )
                        {
                            BUZZER=0;
                            BACKLED=0;
                            BACKLED2=0;
                            E_count++;
                        }
                        else
                        {
                            BUZZER=1;
                            BACKLED=1;
                            BACKLED2=1;
                            E_count++;
                        }
                    }
                    /* EasyCODE ) */
                    E_code=3;
                }
                else
                {
                    if ( E_bat_war==1 )
                    {
                        /* EasyCODE ( 0 
                        Buzzer 1000/50 */
                        if ( E_count>1000 )
                        {
                            BUZZER=0;
                            BACKLED=0;
                            BACKLED2=0;
                            SIDELED=0;
                            E_count=0;
                        }
                        else
                        {
                            if ( E_count>50 )
                            {
                                BUZZER=0;
                                BACKLED=0;
                                BACKLED2=0;
                                SIDELED=0;
                                E_count++;
                            }
                            else
                            {
                                BUZZER=1;
                                BACKLED=1;
                                BACKLED2=1;
                                SIDELED=1;
                                E_count++;
                            }
                        }
                        /* EasyCODE ) */
                        E_code=2;
                    }
                    else
                    {
                        if ( E_CPU==1 )
                        {
                            /* EasyCODE ( 0 
                            Buzzer 100/50 */
                            if ( E_count>100 )
                            {
                                BUZZER=0;
                                BACKLED=0;
                                BACKLED2=0;
                                E_count=0;
                                /* EasyCODE - */
                                E_CPU=0;
                            }
                            else
                            {
                                if ( E_count>50 )
                                {
                                    BUZZER=0;
                                    BACKLED=0;
                                    BACKLED2=0;
                                    E_count++;
                                }
                                else
                                {
                                    BUZZER=1;
                                    BACKLED=1;
                                    BACKLED2=1;
                                    E_count++;
                                }
                            }
                            /* EasyCODE ) */
                            E_code=1;
                        }
                        else
                        {
                            if ( FreqResponse==1 )
                            {
                                /* EasyCODE ( 0 
                                Buzzer 100/50 */
                                if ( E_count>100 )
                                {
                                    SIDELED=0;
                                    E_count=0;
                                    /* EasyCODE - */
                                    E_CPU=0;
                                }
                                else
                                {
                                    if ( E_count>50 )
                                    {
                                        SIDELED=0;
                                        E_count++;
                                    }
                                    else
                                    {
                                        SIDELED=1;
                                        E_count++;
                                    }
                                }
                                /* EasyCODE ) */
                            }
                            else
                            {
                                BUZZER=0;
                                E_count=0;
                                /* EasyCODE ( 0 
                                BACKLEDLED 600/50 */
                                if ( BACKLED_count>275 )
                                {
                                    BACKLED=1;
                                    BACKLED2=1;
                                    BACKLED_count=0;
                                }
                                else
                                {
                                    if ( BACKLED_count>225 )
                                    {
                                        BACKLED=0;
                                        BACKLED2=0;
                                        BACKLED_count++;
                                    }
                                    else
                                    {
                                        if ( BACKLED_count>200 )
                                        {
                                            BACKLED=0;
                                            BACKLED2=1;
                                            BACKLED_count++;
                                        }
                                        else
                                        {
                                            BACKLED=1;
                                            BACKLED2=1;
                                            BACKLED_count++;
                                        }
                                    }
                                }
                                /* EasyCODE ) */
                                SIDELED=1;
                            }
                        }
                    }
                }
            }
            /* EasyCODE ) */
            Update=0;
            Tsload=TMR1;
            Tsload*=0.0000002;
            if ( Tsload>TsloadMax )
            {
                TsloadMax=Tsload;
            }
            BLED=0;
        }
    }
}

/* EasyCODE ) */
/* EasyCODE ( 0 
Functions */
/* EasyCODE ( 0 
void InitPort(void) */
/* EasyCODE F */
void InitPort(void)
{
    DDPCONbits.JTAGEN = 0;
    
    TRISAbits.TRISA0 = 0;//RLED
    TRISAbits.TRISA1 = 0;
    TRISAbits.TRISA2 = 0; //SCL
    TRISAbits.TRISA3 = 0;//SDA
    TRISAbits.TRISA4 = 0;
    TRISAbits.TRISA5 = 0;
    TRISAbits.TRISA6 = 0;
    TRISAbits.TRISA7 = 0; 
    TRISAbits.TRISA9 = 0; //Buzzer
    TRISAbits.TRISA10 = 0; 
    TRISAbits.TRISA14 = 0; //SCL_A
    TRISAbits.TRISA15 = 0; //SDA_A
    
    TRISBbits.TRISB0 = 1;//VLIPO
    TRISBbits.TRISB1 = 1;//CURRENT 
    TRISBbits.TRISB2 = 0;
    TRISBbits.TRISB3 = 0;//DRDY
    TRISBbits.TRISB4 = 0;//CELL2
    TRISBbits.TRISB5 = 0;//CELL1
    //TRISBbits.TRISB6 = 0; //PGC 
    //TRISBbits.TRISB7 = 0;  //PGD
    TRISBbits.TRISB8 = 0;
    TRISBbits.TRISB9 = 0;
    TRISBbits.TRISB10 = 0;
    TRISBbits.TRISB11 = 0; 
    TRISBbits.TRISB12 = 0; 
    TRISBbits.TRISB13 = 0; 
    TRISBbits.TRISB14 = 0; 
    TRISBbits.TRISB15 = 0; //BT_RESET
    
    TRISCbits.TRISC1 = 0;
    TRISCbits.TRISC2 = 0;
    TRISCbits.TRISC3 = 0;
    TRISCbits.TRISC3 = 1;//SD_Data_Out
    //TRISCbits.TRISC12=0; //OSC1
    TRISCbits.TRISC13=0;
    TRISCbits.TRISC14=0;
    //TRISCbits.TRISC15=0; //OSC2
    
    TRISDbits.TRISD0=0; //SD_Data_In
    TRISDbits.TRISD1=0; 
    TRISDbits.TRISD2=1; 
    TRISDbits.TRISD3=0; 
    TRISDbits.TRISD4=0;
    TRISDbits.TRISD5=0;
    TRISDbits.TRISD6=0;
    TRISDbits.TRISD7=0;
    TRISDbits.TRISD8=0; 
    TRISDbits.TRISD9=0; //CS_SD
    TRISDbits.TRISD10=0; //SD_CLK
    TRISDbits.TRISD11=0;
    TRISDbits.TRISD12=0;
    TRISDbits.TRISD13=0;
    TRISDbits.TRISD14=1; //TXDSMX
    TRISDbits.TRISD15=0; //RXDSMX
    
    TRISEbits.TRISE0=0;//LED1
    TRISEbits.TRISE1=0;//LED2
    TRISEbits.TRISE2=0;//OLED
    TRISEbits.TRISE3=0;//GLED
    TRISEbits.TRISE4=0;//LED
    TRISEbits.TRISE5=0;
    TRISEbits.TRISE6=0;
    TRISEbits.TRISE7=0;
    TRISEbits.TRISE8=1;//DRDY
    TRISEbits.TRISE8=1;//INTA
    
    TRISFbits.TRISF0=0;
    TRISFbits.TRISF1=0;
    TRISFbits.TRISF2=1;//TX-IN
    TRISFbits.TRISF3=0;
    TRISFbits.TRISF4=1;//BTTRX
    TRISFbits.TRISF5=0;//BTRX
    TRISFbits.TRISF8=0;//RX-IN
    TRISFbits.TRISF12=1; //BTRTS
    TRISFbits.TRISF13=0; //BTCTS
    
    TRISGbits.TRISG0=0; 
    TRISGbits.TRISG1=0;
    TRISGbits.TRISG2=0;//USBD+
    TRISGbits.TRISG3=0;//USBD-
    TRISGbits.TRISG6=0;//GPSRX
    TRISGbits.TRISG7=0;//SDA/RX-IN
    TRISGbits.TRISG8=0;//SCL/TX-OUT
    TRISGbits.TRISG9=1;//GPSTX
    TRISGbits.TRISG12=0;//LED4
    TRISGbits.TRISG13=0;//BLED
    TRISGbits.TRISG14=0;//LED3
    TRISGbits.TRISG15=0;
    
    //Reset Port A
    LATAbits.LATA0=0;
    LATAbits.LATA1=0;
    LATAbits.LATA2=0;
    LATAbits.LATA3=0;
    LATAbits.LATA4=0;
    LATAbits.LATA5=0;
    LATAbits.LATA6=0;
    LATAbits.LATA7=0;
    LATAbits.LATA9=0;
    LATAbits.LATA10=0;
    LATAbits.LATA14=0;
    LATAbits.LATA15=0;
    
    //Reset Port B
    LATBbits.LATB0 = 0;
    LATBbits.LATB1 = 0;
    LATBbits.LATB2 = 0;
    LATBbits.LATB3 = 0;
    LATBbits.LATB4 = 0;
    LATBbits.LATB5 = 0;
    //LATBbits.LATB6 = 0;
    //LATBbits.LATB7 = 0;
    LATBbits.LATB8 = 0;
    LATBbits.LATB9 = 0;
    LATBbits.LATB10 = 0;
    LATBbits.LATB11 = 0;
    LATBbits.LATB12 = 0;
    LATBbits.LATB13 = 0;
    LATBbits.LATB14 = 0;
    LATBbits.LATB15 = 0;
    
    //Reset Port C
    LATCbits.LATC1=0;
    LATCbits.LATC2=0;
    LATCbits.LATC3=0;
    LATCbits.LATC4=0;
    //LATCbits.LATC12=0;
    LATCbits.LATC13=0;
    LATCbits.LATC14=0;
    //LATCbits.LATC15=0;
    
    //Reset Port D
    LATDbits.LATD0=0;
    LATDbits.LATD1=0;
    LATDbits.LATD2=0;
    LATDbits.LATD3=0;
    LATDbits.LATD4=0;
    LATDbits.LATD5=0;
    LATDbits.LATD6=0;
    LATDbits.LATD7=0;
    LATDbits.LATD8=0;
    LATDbits.LATD9=0;
    LATDbits.LATD10=0;
    LATDbits.LATD11=0;
    LATDbits.LATD12=0;
    LATDbits.LATD13=0;
    LATDbits.LATD14=0;
    LATDbits.LATD15=0;
    
    //Reset Port E
    LATEbits.LATE0=0;
    LATEbits.LATE1=0;
    LATEbits.LATE2=0;
    LATEbits.LATE3=0;
    LATEbits.LATE4=0;
    LATEbits.LATE5=0;
    LATEbits.LATE6=0;
    LATEbits.LATE7=0;
    LATEbits.LATE8=0;
    LATEbits.LATE9=0;
    
    //Reset Port F
    LATFbits.LATF0=0;
    LATFbits.LATF1=0;
    LATFbits.LATF2=0;
    LATFbits.LATF3=0;
    LATFbits.LATF4=0;
    LATFbits.LATF5=0;
    LATFbits.LATF8=0;
    LATFbits.LATF12=0;
    LATFbits.LATF13=0;
    
    //Reset Port G
    LATGbits.LATG0=0;
    LATGbits.LATG1=0;
    LATGbits.LATG2=0;
    LATGbits.LATG3=0;
    LATGbits.LATG6=0;
    LATGbits.LATG7=0;
    LATGbits.LATG8=0;
    LATGbits.LATG9=0;
    LATGbits.LATG12=0;
    LATGbits.LATG13=0;
    LATGbits.LATG14=0;
    LATGbits.LATG15=0;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void InitINT(void) */
/* EasyCODE F */
void InitINT(void)
{
    //Interrupts
    
    
    //INTCON: Interrupt Control Register
    
    //SS0: Single Vector Shadow Register Set bit(1)
    //1 =Single vector is presented with a shadow register set
    //0 =Single vector is not presented with a shadow register set
    INTCONbits.SS0=0;
    
    //MVEC: Multi Vector Configuration bit
    //1 =Interrupt controller configured for multi vectored mode
    //0 =Interrupt controller configured for single vectored mode
    INTCONbits.MVEC=1;
    
    //TPC<2:0>: Interrupt Proximity Timer Control bits
    //111 =Interrupts of group priority 7 or lower start the Interrupt Proximity timer
    //110 =Interrupts of group priority 6 or lower start the Interrupt Proximity timer
    //101 =Interrupts of group priority 5 or lower start the Interrupt Proximity timer
    //100 =Interrupts of group priority 4 or lower start the Interrupt Proximity timer
    //011 =Interrupts of group priority 3 or lower start the Interrupt Proximity timer
    //010 =Interrupts of group priority 2 or lower start the Interrupt Proximity timer
    //001 =Interrupts of group priority 1 start the Interrupt Proximity timer
    //000 =Disables Interrupt Proximity timer
    INTCONbits.TPC=0b000;
    
    //INTxEP: External Interrupt x Edge Polarity Control bit
    //1 =Rising edge
    //0 =Falling edge
    INTCONbits.INT4EP=0;
    INTCONbits.INT3EP=0;
    INTCONbits.INT2EP=0;
    INTCONbits.INT1EP=0;
    
    
    
    //IP0x<2:0>: Interrupt Priority bits
    //111 = Interrupt priority is 7
    //110 = Interrupt priority is 6
    //101 = Interrupt priority is 5
    //100 = Interrupt priority is 4
    //011 = Interrupt priority is 3
    //010 = Interrupt priority is 2
    //001 = Interrupt priority is 1
    //000 = Interrupt is disabled
    
    //IS0x<1:0>: Interrupt Subpriority bits
    //11 = Interrupt subpriority is 3
    //10 = Interrupt subpriority is 2
    //01 = Interrupt subpriority is 1
    //00 = Interrupt subpriority is 0
    IPC1bits.T1IP=0b010;  //Timer 1//2
    IPC1bits.T1IS=0b11;
    
    IPC12bits.U4IP=0b110;//6
    IPC12bits.U4IS=0b11;
    
    IPC8bits.U2IP=0b111;//7
    IPC8bits.U2IS=0b11;
    
    IPC12bits.U6IP=0b101;//5
    IPC12bits.U6IS=0b11;
    
    
    IEC0bits.T1IE=1;	//Timer Interrupt Enable
    IFS0bits.T1IF=0;
    
    IEC2bits.U4RXIE=1;
    IFS2bits.U4RXIF=0;
    
    IEC1bits.U2TXIE=0;
    IEC1bits.U2RXIE=1;
    IFS1bits.U2TXIF=0;
    IFS1bits.U2RXIF=0;
    
    IEC2bits.U6RXIE=1;
    IFS2bits.U6RXIF=0;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void InitTimers(void) */
/* EasyCODE F */
void InitTimers(void)
{
    //SIDL: Stop in Idle Mode bit
    //1 = Discontinue operation when device enters Idle mode
    //0 = Continue operation even in Idle mode
    T1CONbits.SIDL=0;
    
    //TWDIS: Asynchronous Timer Write Disable bit
    //1 = Writes to TMR1 are ignored until pending write operation completes
    //0 = Back-to-back writes are enabled (Legacy Asynchronous Timer functionality)
    T1CONbits.TWDIS=0;
    
    //TCKPS<1:0>: Timer Input Clock Prescale Select bits
    //11 = 1:256 prescale value
    //10 = 1:64 prescale value
    //01 = 1:8 prescale value
    //00 = 1:1 prescale value
    T1CONbits.TCKPS=0b01;
    
    //TCS: Timer Clock Source Select bit
    //1 = External clock from TxCKI pin
    //0 = Internal peripheral clock
    T1CONbits.TCS=0;
    
    //PR<15:0>: Period Register bits
    //These bits represent the complete 16-bit period match.
    //PR1=10000;  //500Hz
    //dt=0.002;
    PR1=12500;  //400Hz
    dt=0.0025;
    //PR1=15000;  //333Hz
    //dt=0.003;
    //PR1=20000;  //250Hz
    //dt=0.004;
    
    //ON: Timer On bit(1)
    //1 = Timer is enabled
    //0 = Timer is disabled
    T1CONbits.ON=1;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void InitADC(void) */
/* EasyCODE F */
void InitADC(void)
{
    /*SIDL: Stop in Idle Mode bit
    1 = Discontinue module operation when device enters Idle mode
    0 = Continue module operation in Idle mode*/
    AD1CON1bits.SIDL=0;
    
    /*FORM<2:0>: Data Output Format bits
    011 = Signed Fractional 16-bit (DOUT = 0000 0000 0000 0000 sddd dddd dd00 0000)
    010 = Fractional 16-bit (DOUT = 0000 0000 0000 0000 dddd dddd dd00 0000)
    001 = Signed Integer 16-bit (DOUT = 0000 0000 0000 0000 ssss sssd dddd dddd)
    000 = Integer 16-bit (DOUT = 0000 0000 0000 0000 0000 00dd dddd dddd)
    111 = Signed Fractional 32-bit (DOUT = sddd dddd dd00 0000 0000 0000 0000)
    110 = Fractional 32-bit (DOUT = dddd dddd dd00 0000 0000 0000 0000 0000)
    101 = Signed Integer 32-bit (DOUT = ssss ssss ssss ssss ssss sssd dddd dddd)
    100 = Integer 32-bit (DOUT = 0000 0000 0000 0000 0000 00dd dddd dddd)*/
    AD1CON1bits.FORM=0b000;
    
    /*SSRC<2:0>: Conversion Trigger Source Select bits
    111 = Internal counter ends sampling and starts conversion (auto convert)
    110 = Reserved
    101 = Reserved
    100 = Reserved
    011 = Reserved
    010 = Timer3 period match ends sampling and starts conversion
    001 = Active transition on INT0 pin ends sampling and starts conversion
    000 = Clearing SAMP bit ends sampling and starts conversion*/
    AD1CON1bits.SSRC=0b111;
    
    /*CLRASAM: Stop Conversion Sequence bit (when the first ADC interrupt is generated)
    1 = Stop conversions when the first ADC interrupt is generated. Hardware clears the ASAM bit when the ADC interrupt is generated.
    0 = Normal operation, buffer contents will be overwritten by the next conversion sequence*/
    AD1CON1bits.CLRASAM=0;
    
    /*ASAM: ADC Sample Auto-Start bit
    1 = Sampling begins immediately after last conversion completes; SAMP bit is automatically set
    0 = Sampling begins when SAMP bit is set*/
    AD1CON1bits.ASAM=1;
    
    /*SAMP: ADC Sample Enable bit
    1 = The ADC SHA is sampling
    0 = The ADC sample/hold amplifier is holding
    When ASAM = 0, writing ?1? to this bit starts sampling.
    When SSRC = 000, writing ?0? to this bit will end sampling and start conversion.*/
    AD1CON1bits.SAMP=0;
    
    /*DONE: Analog-to-Digital Conversion Status bit(2)
    1 = Analog-to-digital conversion is done
    0 = Analog-to-digital conversion is not done or has not started
    Clearing this bit will not affect any operation in progress.*/
    AD1CON1bits.DONE=0;
    
    /*VCFG<2:0>: Voltage Reference Configuration bits
    ADC VR+
    ADC VR-
    000		AVDD				AVSS
    001		External VREF+ pin		AVSS
    010		AVDD				External VREF- pin
    011		External VREF+ pin		External VREF- pin
    1xx		AVDD				AVSS*/
    AD1CON2bits.VCFG=0b000;
    
    /*OFFCAL: Input Offset Calibration Mode Select bit
    1 = Enable Offset Calibration mode
    VINH and VINL of the SHA are connected to VR-
    0 = Disable Offset Calibration mode
    The inputs to the SHA are controlled by AD1CHS or AD1CSSL*/
    AD1CON2bits.OFFCAL=0;
    
    
    
    /*SMPI<3:0>: Sample/Convert Sequences Per Interrupt Selection bits
    1111 = Interrupts at the completion of conversion for each 16th sample/convert sequence
    1110 = Interrupts at the completion of conversion for each 15th sample/convert sequence
    ?
    ?
    ?
    0001 = Interrupts at the completion of conversion for each 2nd sample/convert sequence
    0000 = Interrupts at the completion of conversion for each sample/convert sequence*/
    AD1CON2bits.SMPI=0b0111;
    
    /*BUFM: ADC Result Buffer Mode Select bit
    1 = Buffer configured as two 8-word buffers, ADC1BUF(7...0), ADC1BUF(15...8)
    0 = Buffer configured as one 16-word buffer ADC1BUF(15...0.)*/
    AD1CON2bits.BUFM=0;
    
    /*ALTS: Alternate Input Sample Mode Select bit
    1 = Uses MUX A input multiplexer settings for first sample, then alternates between MUX B and
    MUX A input multiplexer settings for all subsequent samples
    0 = Always use MUX A input multiplexer settings*/
    AD1CON2bits.ALTS=0;
    
    /*ADRC: ADC Conversion Clock Source bit
    1 = ADC internal RC clock
    0 = Clock derived from Peripheral Bus Clock (PBCLK)*/
    AD1CON3bits.ADRC=0;
    
    /*SAMC<4:0>: Auto-sample Time bits
    11111 = 31 TAD
    ?
    ?
    ?
    00001 = 1 TAD
    00000 = 0 TAD (Not allowed)*/
    AD1CON3bits.SAMC=0b00010;
    
    /*ADCS<7:0>: ADC Conversion Clock Select bits(1)
    11111111 = TPB ? 2 ? (ADCS<7:0> + 1) = 512 ? TPB = TAD
    ?
    ?
    ?
    00000001 = TPB ? 2 ? (ADCS<7:0> + 1) = 4 ? TPB = TAD
    00000000 = TPB ? 2 ? (ADCS<7:0> + 1) = 2 ? TPB = TAD*/
    AD1CON3bits.ADCS=0b00000001;
    
    /*CH0NB: Negative Input Select bit for MUX B
    1 = Channel 0 negative input is AN1
    0 = Channel 0 negative input is VR-*/
    AD1CHSbits.CH0NB=0;
    
    /*CH0SB<3:0>: Positive Input Select bits for MUX B
    1111 =Channel 0 positive input is AN15
    1110 =Channel 0 positive input is AN14
    1101 =Channel 0 positive input is AN13
    ?
    ?
    ?
    0001 = Channel 0 positive input is AN1
    0000 = Channel 0 positive input is AN0*/
    AD1CHSbits.CH0SB=0b0000;
    
    /*CH0NA: Negative Input Select bit for MUX A Multiplexer Setting
    1 = Channel 0 negative input is AN1
    0 = Channel 0 negative input is VR-*/
    AD1CHSbits.CH0NA=0;
    
    /*CH0SA<3:0>: Positive Input Select bits for MUX A Multiplexer Setting
    1111 = Channel 0 positive input is AN15
    1110 = Channel 0 positive input is AN14
    1101 = Channel 0 positive input is AN13
    ?
    ?
    ?
    0001 = Channel 0 positive input is AN1
    0000 = Channel 0 positive input is AN0*/
    AD1CHSbits.CH0SA=0b0000;
    
    /*PCFG<15:0>: Analog Input Pin Configuration Control bits
    1 = Analog input pin in Digital mode, port read input enabled, ADC input multiplexer input for this analog input connected to AVss
    0 = Analog input pin in Analog mode, digital port read will return as a ?1? without regard to the voltage on the pin, ADC samples pin voltage*/
    AD1PCFG=0b1111111111111100;
    
    /*CSSL<15:0>: ADC Input Pin Scan Selection bits
    1 = Select ANx for input scan
    0 = Skip ANx for input scan*/
    AD1CSSL=0b0000000000000011;
    
    /*CSCNA: Scan Input Selections for CH0+ SHA Input for MUX A Input Multiplexer Setting bit
    1 = Scan inputs
    0 = Do not scan inputs*/
    AD1CON2bits.CSCNA=1;
    /* EasyCODE - */
    /*ON: ADC Operating Mode bit(1)
    1 = ADC module is operating
    0 = ADC is off*/
    AD1CON1bits.ON=1;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void DelayMs(unsigned int msec) */
/* EasyCODE F */
void DelayMs(unsigned int msec)
{
    unsigned int tWait, tStart; 
              
    tWait=(SYS_FREQ/2000)*msec;        //    SYS_FREQ        (80000000) 
    tStart=ReadCoreTimer();
    while ( (ReadCoreTimer()-tStart)<tWait )
    {
        ;
    }
}

/* EasyCODE ) */
/* EasyCODE ( 0 
cleararray(array[], grote ) */
/* EasyCODE F */
void cleararray(float array[], unsigned int grote )
{
    unsigned int x=0;
    for ( x=0; x<grote; x++ )
    {
        array[x]=0;
    }
}

/* EasyCODE ) */
/* EasyCODE ( 0 
cleararraychar(array[], grote ) */
/* EasyCODE F */
void cleararraychar(unsigned char array[], unsigned int grote )
{
    unsigned int x=0;
    for ( x=0; x<grote; x++ )
    {
        array[x]=0;
    }
}

/* EasyCODE ) */
/* EasyCODE ( 0 
cleararrayint(array[], grote ) */
/* EasyCODE F */
void cleararrayint(unsigned int array[], unsigned int grote )
{
    unsigned int x=0;
    for ( x=0; x<grote; x++ )
    {
        array[x]=0;
    }
}

/* EasyCODE ) */
/* EasyCODE ( 0 
AHRS */
/* EasyCODE ( 0 
MadgwickAHRSupdate */
/* EasyCODE F */
//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float hx, hy;
    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
    
    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
    if ( (mx == 0.0f) && (my == 0.0f) && (mz == 0.0f) )
    {
        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
        return ;
    }
    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
    
    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if ( !((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)) )
    {
        // Normalise accelerometer measurement
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;   
        
        // Normalise magnetometer measurement
        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
        mx *= recipNorm;
        my *= recipNorm;
        mz *= recipNorm;
        
        // Auxiliary variables to avoid repeated arithmetic
        _2q0mx = 2.0f * q0 * mx;
        _2q0my = 2.0f * q0 * my;
        _2q0mz = 2.0f * q0 * mz;
        _2q1mx = 2.0f * q1 * mx;
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _2q0q2 = 2.0f * q0 * q2;
        _2q2q3 = 2.0f * q2 * q3;
        q0q0 = q0 * q0;
        q0q1 = q0 * q1;
        q0q2 = q0 * q2;
        q0q3 = q0 * q3;
        q1q1 = q1 * q1;
        q1q2 = q1 * q2;
        q1q3 = q1 * q3;
        q2q2 = q2 * q2;
        q2q3 = q2 * q3;
        q3q3 = q3 * q3;
        
        // Reference direction of Earth's magnetic field
        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
        _2bx = sqrt(hx * hx + hy * hy);
        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
        _4bx = 2.0f * _2bx;
        _4bz = 2.0f * _2bz;
        
        // Gradient decent algorithm corrective step
        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;
        
        // Apply feedback step
        qDot1 -= beta * s0;
        qDot2 -= beta * s1;
        qDot3 -= beta * s2;
        qDot4 -= beta * s3;
    }
    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * dt;
    q1 += qDot2 * dt;
    q2 += qDot3 * dt;
    q3 += qDot4 * dt;
    
    //q0 += qDot1 * (1.0f / sampleFreq);
    //q1 += qDot2 * (1.0f / sampleFreq);
    //q2 += qDot3 * (1.0f / sampleFreq);
    //q3 += qDot4 * (1.0f / sampleFreq);
    
    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
MadgwickAHRSupdateIMU */
/* EasyCODE F */
//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az)
{
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
    
    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
    
    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if ( !((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)) )
    {
        // Normalise accelerometer measurement
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;   
        
        // Auxiliary variables to avoid repeated arithmetic
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _4q0 = 4.0f * q0;
        _4q1 = 4.0f * q1;
        _4q2 = 4.0f * q2;
        _8q1 = 8.0f * q1;
        _8q2 = 8.0f * q2;
        q0q0 = q0 * q0;
        q1q1 = q1 * q1;
        q2q2 = q2 * q2;
        q3q3 = q3 * q3;
        
        // Gradient decent algorithm corrective step
        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
        s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
        s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;
        
        // Apply feedback step
        qDot1 -= beta * s0;
        qDot2 -= beta * s1;
        qDot3 -= beta * s2;
        qDot4 -= beta * s3;
    }
    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * dt;
    q1 += qDot2 * dt;
    q2 += qDot3 * dt;
    q3 += qDot4 * dt;
    
    //q0 += qDot1 * (1.0f / sampleFreq);
    //q1 += qDot2 * (1.0f / sampleFreq);
    //q2 += qDot3 * (1.0f / sampleFreq);
    //q3 += qDot4 * (1.0f / sampleFreq);
    
    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
invSqrt */
/* EasyCODE F */
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float invSqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;
    i = 0x5f3759df - (i>>1);
    y = *(float*)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

/* EasyCODE ) */
/* EasyCODE ) */
/* EasyCODE ( 0 
biquad */
/* EasyCODE F */
float biquad(float in)
{
    lastin2 =lastin;
    lastin=currin;
    currin=in;
    lastout2=lastout;
    lastout=currout;
    currout=a0*currin + a1*lastin + a2*lastin2 - b1*lastout - b2*lastout2;
    return currout;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void target(void) */
/* EasyCODE F */
void target(void)
{
    float T1, T2, T3, T4, T5, T6, T7, X, Y;
    float dlat, dlon, lat1, lon1, lat2, lon2;
    float A;
    
    lat1=ToRad(GPS.Latitude);
    lon1=ToRad(GPS.Longitude);
    lat2=ToRad(GPS.SetLat);
    lon2=ToRad(GPS.SetLong);
    dlat=lat2-lat1;
    dlon=lon2-lon1;
    
    
    T1=sinf(dlat/2);
    T2=cosf(lat1);
    T3=cosf(lat2);
    T4=sinf(dlon/2);
    T5=(T1*T1+T2*T3*T4*T4);
    T6=(2*atan2(sqrt(T5), sqrt(1-T5)));
    T7=(T6*6371);
    /* EasyCODE - */
    GPS.Target_distance=(T7*1000);
    
    T1=sin(dlon);
    T2=cos(lat2);
    Y=(T1*T2);
    T1=cos(lat1);
    T2=sin(lat2);
    T3=sin(lat1);
    T4=cos(lat2);
    T5=cos(dlon);
    X=(T1*T2-T3*T4*T5);
    A=ToDeg(atan2(Y, X));
    GPS.Target_angle=A;//fmod((A+360), 360);
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    if ( abs(GPS.Target_distance-(T7*1000))<1000 )
    {
    }
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
}

/* EasyCODE ) */
/* EasyCODE ( 0 
matrixMultiplyVector */
/* EasyCODE F */
/* EasyCODE < */
void matrixMultiplyVector(const struct matrix3f * a, const struct vector3f * b, struct vector3f * c)
/* EasyCODE > */
{
    c->x = a->m11 * b->x + a->m12 * b->y + a->m13 * b->z;
    c->y = a->m21 * b->x + a->m22 * b->y + a->m23 * b->z;
    c->z = a->m31 * b->x + a->m32 * b->y + a->m33 * b->z;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
int gpsReadDecimal(float * target, int pos) */
/* EasyCODE F */
int gpsReadDecimal(float * target, int pos)
{
    int endPos, fp;
    for ( endPos = pos; endPos < pos + 6; endPos++ )
    {
        if ( GPS.GPRMC[endPos] == '.' )
        {
            fp = endPos;
        }
        else
        {
            if ( GPS.GPRMC[endPos] == ',' )
            {
                break;
            }
        }
    }
    float val = 0;
    int i = fp;
    for ( i=pos;i<fp;i++ )
    {
        val *= 10;
        val += (GPS.GPRMC[i] - '0');
    }
    if ( endPos - fp > 1 )
    {
        float fpVal = 0;
        for ( i=fp+1;i<endPos;i++ )
        {
            val *= 10;
            val += (GPS.GPRMC[i] - '0');
        }
        for ( i=fp+1;i<endPos;i++ )
        {
            val /= 10;
        }
    }
    *target = val;
    return endPos;
}

/* EasyCODE ) */
/* EasyCODE ) */
/* EasyCODE ( 0 
Interrupts */
/* EasyCODE ( 0 
Timer 1 */
/* EasyCODE F */
void __ISR(_TIMER_1_VECTOR, IPL2SOFT) Timer1Handler(void)
{
    BLED=1;
    FRONTLED=1;
    /* EasyCODE ( 0 
    PID */
    /* EasyCODE ( 0 
    AHRS */
    I2C2BRG=44;	//400kHz @ 40Mhz Bus Clock //44
    MPU(&Gyro.X, &Gyro.Y, &Gyro.Z, &Acc.X, &Acc.Y, &Acc.Z, &Gyro.TEMP);
    
    Gyro.X-=Gyro.Xoffset;
    Gyro.Y-=Gyro.Yoffset;
    Gyro.Z-=Gyro.Zoffset;
    
    Acc.X-=Acc.Xoffset;
    Acc.Y-=Acc.Yoffset;
    Acc.Z-=Acc.Zoffset;
    
    Gyro.X=ToRad(Gyro.X);
    Gyro.Y=ToRad(Gyro.Y);
    Gyro.Z=ToRad(Gyro.Z);
    
    HMC5883L(&Compass.X, &Compass.Y, &Compass.Z);
    /* EasyCODE - */
    Compass.X-=Compass.Xoffset;
    Compass.Y-=Compass.Yoffset;
    Compass.Z-=Compass.Zoffset;
    
    Compass.X=Compass.X*Compass.X1+Compass.X*Compass.X2+Compass.X*Compass.X3;
    Compass.Y=Compass.Y*Compass.Y1+Compass.Y*Compass.Y2+Compass.Y*Compass.Y3;
    Compass.Z=Compass.Z*Compass.Z1+Compass.Z*Compass.Z2+Compass.Z*Compass.Z3;
    /* EasyCODE - */
    Compass.X*=-1;
    //Compass.Y*=-1;
    Compass.Z*=-1;
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    EasyCODE_COMMENT_START EasyCODE ( 0 
    Baro EasyCODE_COMMENT_END
    if ( Baro.Timer==4 )
    {
        Baro.Timer=1;
        if ( Baro.TP==0 )
        {
            MS5611(&Baro.D2);
            MS5611Command(MS5611_D1_OSR_4096);
            Baro.TP=1;
            Baro.dT=(Baro.D2-Baro.C5*256);
            Baro.TEMP=2000+(Baro.dT*(Baro.C6/powf(2, 23)));
            Baro.OFF=(long long)Baro.C2*65536+((long long)Baro.C4*(long long)Baro.dT)/128;
            Baro.SENS=(long long)Baro.C1*32768+((long long)Baro.C3*(long long)Baro.dT)/256;
        }
        else
        {
            MS5611(&Baro.D1);
            MS5611Command(MS5611_D2_OSR_4096);
            Baro.TP=0;
            Baro.P=(Baro.D1*Baro.SENS/powf(2, 21)-Baro.OFF)/32768;
            Baro.Pressure=Baro.P;
            Baro.Pressure/=100;
            Baro.Temperature=Baro.TEMP;
            Baro.Temperature/=100;
            //Baro.Altitude=44330*(1-powf((Baro.Pressure/1013.25),(1/5.255)));
            if ( (Throttle+ThrottleStick)>=Motor.MinRpm )
            {
            }
            else
            {
                Baro.StartPressure=Baro.Pressure;
            }
            Baro.Altitude=(-1*(   (8.3144621*(Baro.Temperature+273.15))  / (0.028967*9.81)  )*logf(Baro.Pressure/Baro.StartPressure));
            Baro.AltitudeFilter=Baro.AltitudeFilter+(LPFGain*(Baro.Altitude-Baro.AltitudeFilter));
            Baro.AltitudeSpeed=(Baro.AltitudeFilter-Baro.AltitudeOld)/(dt*8);;
            Baro.AltitudeOld=Baro.AltitudeFilter;
        }
    }
    else
    {
        Baro.Timer++;
    }
    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    MadgwickAHRSupdate(Gyro.X, Gyro.Y, Gyro.Z, Acc.X, Acc.Y, Acc.Z, Compass.X, Compass.Y, Compass.Z);
    //MadgwickAHRSupdateIMU(Gyro.X, Gyro.Y, Gyro.Z, Acc.X, Acc.Y, Acc.Z);
    
    AngleX = atan2(2 * (q0*q1 + q2*q3), 1 - 2 *(q1*q1 + q2*q2));
    AngleY = asin(2 * (q0*q2 - q1*q3));
    AngleZ = atan2(2 * (q0*q3 + q1*q2), 1 - 2 *(q2*q2 + q3*q3));
    
    g0 = 2 * (q1 * q3 - q0 * q2);
    g1 = 2 * (q0 * q1 + q2 * q3);
    g2 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
    
    /*Acc.gX=Acc.X-g0;
    Acc.gY=Acc.Y-g1;
    Acc.gZ=Acc.Z-g2;*/
    //Acc.G=sqrt(Acc.gX*Acc.gX+Acc.gY*Acc.gY+Acc.gZ*Acc.gZ)
    /* EasyCODE - */
    sintheta = sinf(AngleY);
    costheta = cosf(AngleY);
    sinomega = sinf(AngleX);
    cosomega = cosf(AngleX);
    accNormal.x = (Acc.X-g0);
    accNormal.y = (Acc.Y-g1);
    accNormal.z = (Acc.Z-g2);
    
    
    rotationMatrix.m11 = costheta;
    rotationMatrix.m12 = sinomega * sintheta ;
    rotationMatrix.m13 = cosomega * sintheta;
    rotationMatrix.m21 = 0.0f;
    rotationMatrix.m22 = cosomega;
    rotationMatrix.m23 = -sinomega;
    rotationMatrix.m31 = -sintheta;
    rotationMatrix.m32 = sinomega * costheta;
    rotationMatrix.m33 = cosomega * costheta;
    
    matrixMultiplyVector(&rotationMatrix,&accNormal,&accRotated);
    
    Acc.gX = accRotated.x;
    Acc.gY = accRotated.y;
    Acc.gZ = accRotated.z;
    /* EasyCODE - */
    Acc.gZ_filter=Acc.gZ_filter+(0.075*(Acc.gZ-Acc.gZ_filter)); //0.1=6.5Hz 0.05=3.25Hz, 0.075=5Hz
    Speed=Speed+(dt*(Acc.gZ_filter*9.81+(SpeedGain*(Baro.AltitudeSpeed-Speed))));
    
    Altitude=Altitude+(dt*(Speed+(AltitudeGain*(Baro.Altitude-Altitude))));
    AltSpeed=(Altitude-AltitudeOld)/dt;
    AltitudeOld=Altitude;
    
    AngleX=ToDeg(AngleX);
    AngleY=ToDeg(AngleY);
    AngleZ=ToDeg(AngleZ);
    
    Gyro.X=ToDeg(Gyro.X);
    Gyro.Y=ToDeg(Gyro.Y);
    Gyro.Z=ToDeg(Gyro.Z);
    
    
    Compass.Angle=atan2(Compass.X, Compass.Y);
    Compass.Angle=ToDeg(Compass.Angle);
    Compass.Angle*=-1;
    AngleZ*=-1;
    /* EasyCODE - */
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    AngleZ-=declination;
    Compass.Angle-=declination;
    if ( AngleZ>=-90.0f )
    {
        AngleZ-=90;
    }
    else
    {
        AngleZ+=270;
    }
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    /* EasyCODE ) */
    if ( Mode==2 )
    {
        if ( GPS.Status==1 )
        {
            /* EasyCODE ( 0 
            PID GPS */
            if ( Throttle>=0000 && (AltitudeHoldOn==1 || AltitudeControlOn==1) )
            {
                /* EasyCODE ( 0 
                PIDD X */
                GPS.X_Error=sinf((ToRad(GPS.Target_angle)-ToRad(AngleZ))) *GPS.Target_distance;
                //P-factor
                PvalueDX=GPS.X_Error*KrN;
                //I-factor
                IvalueDX=IvalueDX+((GPS.X_Error*KiN)*dt);
                //D-factor
                DFilterDX=DFilterDX+(LPFGainD*(GPS.X_Error-DFilterDX));
                DvalueDX=(DFilterDX-OldErrorDX)/dt;
                DvalueDX=DvalueDX*KdN;
                OldErrorDX=DFilterDX;
                limit=10;
                if ( IvalueDX>limit )
                {
                    IvalueDX=limit;
                }
                else
                {
                    if ( IvalueDX<-limit )
                    {
                        IvalueDX=-limit;
                    }
                }
                if ( KiN==0 )
                {
                    IvalueDX=0;
                }
                PIDDX=((PvalueDX)+(IvalueDX)+(DvalueDX));
                if ( PIDDX>10 )
                {
                    PIDDX=10;
                }
                else
                {
                    if ( PIDDX<-10 )
                    {
                        PIDDX=-10;
                    }
                }
                /* EasyCODE_STRUCTURED_COMMENT_START */
                /*
                PIDDX*=-1;
                */
                /* EasyCODE_STRUCTURED_COMMENT_END */
                /* EasyCODE ) */
                /* EasyCODE ( 0 
                PIDD Y */
                GPS.Y_Error=cosf((ToRad(GPS.Target_angle)-ToRad(AngleZ))) *GPS.Target_distance;
                
                //P-factor
                PvalueDY=GPS.Y_Error*KrN;
                //I-factor
                IvalueDY=IvalueDY+((GPS.Y_Error*KiN)*dt);
                //D-factor
                DFilterDY=DFilterDY+(LPFGainD*(GPS.Y_Error-DFilterDY));
                DvalueDY=(DFilterDY-OldErrorDY)/dt;
                DvalueDY=DvalueDY*KdN;
                OldErrorDY=DFilterDY;
                limit=10;
                if ( IvalueDY>limit )
                {
                    IvalueDY=limit;
                }
                else
                {
                    if ( IvalueDY<-limit )
                    {
                        IvalueDY=-limit;
                    }
                }
                if ( KiN==0 )
                {
                    IvalueDY=0;
                }
                PIDDY=((PvalueDY)+(IvalueDY)+(DvalueDY));
                if ( PIDDY>10 )
                {
                    PIDDY=10;
                }
                else
                {
                    if ( PIDDY<-10 )
                    {
                        PIDDY=-10;
                    }
                }
                /* EasyCODE ) */
                /* EasyCODE_STRUCTURED_COMMENT_START */
                /*
                if ( (AltitudeHoldOn==1 && GPS.Target_distance>1) )
                {
                    SetpointX=PIDDX;
                    SetpointY=PIDDY;
                }
                */
                /* EasyCODE_STRUCTURED_COMMENT_END */
                SetpointX=PIDDX;
                SetpointY=PIDDY;
            }
            else
            {
                PvalueDX=0;
                IvalueDX=0;
                DvalueDX=0;
                OldErrorDX=0;
                //PIDDX=0;
                
                
                PvalueDY=0;
                IvalueDY=0;
                DvalueDY=0;
                OldErrorDY=0;
                //PIDDY=0;
            }
            /* EasyCODE ) */
        }
        /* EasyCODE ( 0 
        PID Altitude */
        if ( Throttle>=Motor.MinRpm && (AltitudeControlOn==1) )
        {
            ErrorA=(SetpointAFilter-Altitude);
            //P-factor
            PvalueA=ErrorA*KrA;
            //I-factor
            IvalueA=IvalueA+((ErrorA*KiA)*dt);
            //D-factor
            DFilterA=DFilterA+(LPFGainA*(ErrorA-DFilterA));
            DvalueA=(DFilterA-OldErrorA)/dt;
            DvalueA=DvalueA*KdA;
            OldErrorA=DFilterA;
            limit=3;
            if ( IvalueA>limit )
            {
                IvalueA=limit;
            }
            else
            {
                limit=-3;
                if ( IvalueA<limit )
                {
                    IvalueA=limit;
                }
            }
            SetpointAspeed=((PvalueA)+(IvalueA)+(DvalueA));
            if ( SetpointAspeed>4 )
            {
                SetpointAspeed=4;
            }
            else
            {
                if ( SetpointAspeed<-3 )
                {
                    SetpointAspeed=-3;
                }
            }
        }
        else
        {
            ErrorA=0;
            PvalueA=0;
            IvalueA=0;
            DvalueA=0;
        }
        /* EasyCODE ) */
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        EasyCODE_COMMENT_START EasyCODE ( 0 
        PID Altitude EasyCODE_COMMENT_END
        if ( Throttle>=Motor.MinRpm && (AltitudeHoldOn==1 || AltitudeControlOn==1) )
        {
            ErrorA=(SetpointA-Altitude);
            //P-factor
            PvalueA=ErrorA*KrA;
            //I-factor
            IvalueA=IvalueA+((ErrorA*KiA)*dt);
            //D-factor
            DFilterA=DFilterA+(LPFGainA*(ErrorA-DFilterA));
            DvalueA=(DFilterA-OldErrorA)/dt;
            DvalueA=DvalueA*KdA;
            OldErrorA=DFilterA;
            limit=4900;
            if ( IvalueA>limit )
            {
                IvalueA=limit;
            }
            else
            {
                limit=4400;
                if ( IvalueA<limit )
                {
                    IvalueA=limit;
                }
            }
            PIDA=(signed long)((PvalueA)+(IvalueA)+(DvalueA));
            if ( PIDA>4900 )
            {
                PIDA=4900;
            }
            else
            {
                if ( PIDA<4200 )
                {
                    PIDA=4200;
                }
            }
            if ( (AltitudeHoldOn==1 || AltitudeControlOn==1) )
            {
                ThrottleStick=PIDA;
            }
        }
        else
        {
            ErrorA=0;
            PvalueA=0;
            IvalueA=ThrottleStick;
            DvalueA=0;
            PIDA=0;
        }
        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
        EasyCODE_COMMENT_START EasyCODE ( 0 
        PIDZ Angle EasyCODE_COMMENT_END
        if ( (Throttle+ThrottleStick)>=1500 )
        {
            if ( AngleX>30 || AngleY>30 )
            {
                SetpointZspeed=0;
            }
            P1=AngleZ;
            P2=AngleZ-360;
            P3=AngleZ+360;
            if ( abs(SetpointZ-P1)<abs(SetpointZ-P2) )
            {
                if ( abs(SetpointZ-P1)<abs(SetpointZ-P3) )
                {
                    ErrorZS=(SetpointZ-P1);
                }
                else
                {
                    ErrorZS=(SetpointZ-P3);
                }
            }
            else
            {
                if ( abs(SetpointZ-P2)<abs(SetpointZ-P3) )
                {
                    ErrorZS=(SetpointZ-P2);
                }
                else
                {
                    ErrorZS=(SetpointZ-P3);
                }
            }
            if ( ErrorZS>90 )
            {
                ErrorZS=90;
            }
            else
            {
                if ( ErrorZS<-90 )
                {
                    ErrorZS=-90;
                }
            }
            //P-factor
            PvalueZS=ErrorZS*KrZS;
            //I-factor
            IvalueZS=IvalueZS+(ErrorZS*KiZS*dt);
            //D-factor
            DvalueZS=Gyro.Z*KdZS*1;
            limit=120;
            if ( IvalueZS>limit )
            {
                IvalueZS=limit;
            }
            else
            {
                if ( IvalueZS<(limit*-1) )
                {
                    IvalueZS=(limit*-1);
                }
            }
            if ( KiZS==0 )
            {
                IvalueZS=0;
            }
            SetpointZspeed=(PvalueZS+IvalueZS+DvalueZS);
            if ( SetpointZspeed>120 )
            {
                SetpointZspeed=120;
            }
            else
            {
                if ( SetpointZspeed<-120 )
                {
                    SetpointZspeed=-120;
                }
            }
        }
        else
        {
            IvalueZS=0;
            SetpointZspeed=0;
        }
        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
        /* EasyCODE ( 0 
        PIDX Angle */
        if ( Throttle>=Motor.MinRpm )
        {
            //Error=Setpoint-current Angle
            ErrorXS=(SetpointX-AngleX);
            /* EasyCODE_STRUCTURED_COMMENT_START */
            /*
            if ( ErrorXS>AngleLimit )
            {
                ErrorXS=AngleLimit;
            }
            else
            {
                if ( ErrorXS<(-1*AngleLimit) )
                {
                    ErrorXS=(-1*AngleLimit);
                }
            }
            */
            /* EasyCODE_STRUCTURED_COMMENT_END */
            //P-factor
            PvalueXS=ErrorXS*KrXS;
            //I-factor
            IvalueXS=IvalueXS+(ErrorXS*KiXS*dt);
            //D-factor
            DvalueXS=Gyro.X*KdXS*-1;
            limit=450;
            if ( IvalueXS>limit )
            {
                IvalueXS=limit;
            }
            else
            {
                if ( IvalueXS<(limit*-1) )
                {
                    IvalueXS=(limit*-1);
                }
            }
            if ( KiXS==0 )
            {
                IvalueXS=0;
            }
            SetpointXspeed=(PvalueXS+IvalueXS+DvalueXS+SignalA);
            if ( SetpointXspeed>250 )
            {
                SetpointXspeed=250;
            }
            else
            {
                if ( SetpointXspeed<-250 )
                {
                    SetpointXspeed=-250;
                }
            }
        }
        else
        {
            IvalueXS=0;
            SetpointXspeed=0;
        }
        /* EasyCODE ) */
        /* EasyCODE ( 0 
        PIDY Angle */
        if ( Throttle>=Motor.MinRpm )
        {
            //Error=Setpoint-current Angle
            ErrorYS=(SetpointY-AngleY);
            /* EasyCODE_STRUCTURED_COMMENT_START */
            /*
            if ( ErrorYS>AngleLimit )
            {
                ErrorYS=AngleLimit;
            }
            else
            {
                if ( ErrorYS<(-1*AngleLimit) )
                {
                    ErrorYS=(-1*AngleLimit);
                }
            }
            */
            /* EasyCODE_STRUCTURED_COMMENT_END */
            //P-factor
            PvalueYS=ErrorYS*KrYS;
            //I-factor
            IvalueYS=IvalueYS+(ErrorYS*KiYS*dt);
            //D-factor
            DvalueYS=Gyro.Y*KdYS*-1;
            limit=450;
            if ( IvalueYS>limit )
            {
                IvalueYS=limit;
            }
            else
            {
                if ( IvalueYS<(limit*-1) )
                {
                    IvalueYS=(limit*-1);
                }
            }
            if ( KiYS==0 )
            {
                IvalueYS=0;
            }
            SetpointYspeed=(PvalueYS+IvalueYS+DvalueYS);
            if ( SetpointYspeed>250 )
            {
                SetpointYspeed=250;
            }
            else
            {
                if ( SetpointYspeed<-250 )
                {
                    SetpointYspeed=-250;
                }
            }
        }
        else
        {
            IvalueYS=0;
            SetpointYspeed=0;
        }
        /* EasyCODE ) */
    }
    /* EasyCODE ( 0 
    PID Altitude Speed */
    if ( Throttle>=Motor.MinRpm && (AltitudeHoldOn==1 || AltitudeControlOn==1) )
    {
        ErrorAS=(SetpointAspeed-Speed);
        /* EasyCODE - */
        //P-factor
        PvalueAS=ErrorAS*KrAS;
        //I-factor
        IvalueAS_Old=IvalueAS;
        IvalueAS=IvalueAS+((ErrorAS*KiAS)*dt);
        //D-factor
        DFilterAS=DFilterAS+(LPFGainAS*(ErrorAS-DFilterAS));
        DvalueAS=(DFilterAS-OldErrorAS)/dt;
        DvalueAS=DvalueAS*KdAS;
        OldErrorAS=DFilterAS;
        limit=5900;
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        if ( IvalueAS>limit )
        {
            IvalueAS=limit;
        }
        else
        {
            limit=3600;
            if ( IvalueAS<limit )
            {
                IvalueAS=limit;
            }
        }
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
        PIDA=(signed long)((PvalueAS)+(IvalueAS)+(DvalueAS)+(SignalAS));
        if ( PIDA>5900 )
        {
            PIDA=5900;
            IvalueAS=IvalueAS_Old;
        }
        else
        {
            if ( PIDA<3300 )
            {
                PIDA=3300;
                IvalueAS=IvalueAS_Old;
            }
        }
        if ( (AltitudeHoldOn==1 || AltitudeControlOn==1) )
        {
            ThrottleStick=PIDA;
        }
    }
    else
    {
        ErrorAS=0;
        PvalueAS=0;
        IvalueAS=ThrottleStick;
        DvalueAS=0;
        PIDA=0;
    }
    /* EasyCODE ) */
    /* EasyCODE ( 0 
    PID Z Angular speed */
    if ( Throttle>=Motor.MinRpm )
    {
        //Error=Setpoint-current Angle
        //ErrorZ=((SetpointZspeed*-1)-(Gyro.Z));
        
        ErrorZ=ErrorZ+(0.25*(((SetpointZspeed*-1)-(Gyro.Z))-ErrorZ));
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        if ( ErrorZ>100 )
        {
            ErrorZ=100;
        }
        else
        {
            if ( ErrorZ<-100 )
            {
                ErrorZ=-100;
            }
        }
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
        //P-factor
        PvalueZ=ErrorZ*KrZ;
        //I-factor
        IvalueZ_Old=IvalueZ;
        IvalueZ=IvalueZ+((ErrorZ*KiZ)*dt);
        //D-factor
        IDvalueZ=IDvalueZ+(DvalueZ*dt);
        DvalueZ=((ErrorZ*KdZ)-IDvalueZ);
        DvalueZ*=Dfilter;
        //Remember Error
        OldErrorZ=ErrorZ;
        limit=1000;
        if ( IvalueZ>limit )
        {
            IvalueZ=limit;
        }
        else
        {
            if ( IvalueZ<(limit*-1) )
            {
                IvalueZ=(limit*-1);
            }
        }
        if ( IDvalueZ>limit )
        {
            IDvalueZ=limit;
        }
        else
        {
            if ( IDvalueZ<(limit*-1) )
            {
                IDvalueZ=(limit*-1);
            }
        }
        if ( KiZ==0 )
        {
            IvalueZ=0;
        }
        DvalueZ=0;
        /* EasyCODE - */
        PIDZ=(signed long)((PvalueZ)+(IvalueZ)+(DvalueZ)+(SignalZ));
        if ( PIDZ>1500 )
        {
            PIDZ=1500;
            //IvalueZ=IvalueZ_Old;
        }
        else
        {
            if ( PIDZ<-1500 )
            {
                PIDZ=-1500;
                //IvalueZ=IvalueZ_Old;
            }
        }
    }
    else
    {
        IvalueZ=0;
        IDvalueZ=0;
    }
    /* EasyCODE ) */
    /* EasyCODE ( 0 
    PID X Angular speed */
    if ( Throttle>=Motor.MinRpm )
    {
        //Error=Setpoint-current Angle
        ErrorX=(SetpointXspeed-Gyro.X);
        //ErrorX=0-In;
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        if ( ErrorX>450 )
        {
            ErrorX=450;
        }
        else
        {
            if ( ErrorX<-450 )
            {
                ErrorX=-450;
            }
        }
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
        //P-factor
        PvalueX=ErrorX*KrX;
        //I-factor
        IvalueXold=IvalueX;
        IvalueX=IvalueX+((ErrorX*KiX)*dt);
        //D-factor
        IDvalueX=IDvalueX+(DvalueX*dt);
        DvalueX=((ErrorX*KdX)-IDvalueX);
        DvalueX*=Dfilter;
        //Remember Error
        OldErrorX=ErrorX;
        limit=2000;
        if ( IvalueX>limit )
        {
            IvalueX=limit;
        }
        else
        {
            if ( IvalueX<(limit*-1) )
            {
                IvalueX=(limit*-1);
            }
        }
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        if ( (Motor.B==Motor.MaxRpm&&Motor.R==Motor.MaxRpm) )
        {
            if ( IvalueX<0 )
            {
                if ( IvalueX<IvalueXold )
                {
                    IvalueX=IvalueXold;
                }
            }
            else
            {
                if ( IvalueX>IvalueXold )
                {
                    IvalueX=IvalueXold;
                }
            }
        }
        else
        {
            if ( (Motor.F==Motor.MaxRpm&&Motor.L==Motor.MaxRpm) )
            {
                if ( IvalueX<0 )
                {
                    if ( IvalueX<IvalueXold )
                    {
                        IvalueX=IvalueXold;
                    }
                }
                else
                {
                    if ( IvalueX>IvalueXold )
                    {
                        IvalueX=IvalueXold;
                    }
                }
            }
        }
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
        if ( IDvalueX>limit )
        {
            IDvalueX=limit;
        }
        else
        {
            if ( IDvalueX<(limit*-1) )
            {
                IDvalueX=(limit*-1);
            }
        }
        if ( KiX==0 )
        {
            IvalueX=0;
        }
        DvalueX=0;
        /* EasyCODE - */
        PIDX=(signed long)((PvalueX)+(IvalueX)+(DvalueX)+(SignalS));
        /* EasyCODE - */
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        //PIDX=(signed long)(SetpointXspeed*0.666667);
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
}
    else
    {
        IvalueX=0;
        IDvalueX=0;
        PIDX=0;
    }
    /* EasyCODE ) */
    /* EasyCODE ( 0 
    PID Y Angular speed */
    if ( Throttle>=Motor.MinRpm )
    {
        //Error=Setpoint-current Angle
        ErrorY=(SetpointYspeed-Gyro.Y);
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        if ( ErrorY>450 )
        {
            ErrorY=450;
        }
        else
        {
            if ( ErrorY<-450 )
            {
                ErrorY=-450;
            }
        }
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
        //P-factor
        PvalueY=ErrorY*KrY;
        //I-factor
        IvalueY=IvalueY+((ErrorY*KiY)*dt);
        //D-factor
        IDvalueY=IDvalueY+(DvalueY*dt);
        DvalueY=((ErrorY*KdY)-IDvalueY);
        DvalueY*=Dfilter;
        //Remember Error
        OldErrorY=ErrorY;
        limit=2000;
        if ( IvalueY>limit )
        {
            IvalueY=limit;
        }
        else
        {
            if ( IvalueY<(limit*-1) )
            {
                IvalueY=(limit*-1);
            }
        }
        if ( IDvalueY>limit )
        {
            IDvalueY=limit;
        }
        else
        {
            if ( IDvalueY<(limit*-1) )
            {
                IDvalueY=(limit*-1);
            }
        }
        if ( KiY==0 )
        {
            IvalueY=0;
        }
        DvalueY=0;
        /* EasyCODE - */
        PIDY=(signed long)((PvalueY)+(IvalueY)+(DvalueY));
    }
    else
    {
        IvalueY=0;
        IDvalueY=0;
        PIDY=0;
    }
    /* EasyCODE ) */
    if ( Throttle<Motor.MinRpm )
    {
        Motor.F=0;
        Motor.B=0;
        Motor.L=0;
        Motor.R=0;
    }
    else
    {
        /* EasyCODE ( 0 
        Motor Limit */
        RPMXF=(0.56*PIDX)-(1.31*PIDY)-(1.31*PIDZ); //Front Right
        RPMYL=(0.56*PIDX)+(1.31*PIDY)+(1.31*PIDZ); //Front Left
        RPMYR=(-1.08*PIDX)-(1*PIDY)+(1*PIDZ); //Back Right
        RPMXB=(-1.08*PIDX)+(1*PIDY)-(1*PIDZ);//Back Left
        /* EasyCODE - */
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        RPMXF=PIDX-PIDY-PIDZ; //Front Right
        RPMYL=PIDX+PIDY+PIDZ; //Front Left
        RPMYR=(-1*PIDX)-(-1*PIDY)-PIDZ; //Back Right
        RPMXB=(-1*PIDX)+(-1*PIDY)+PIDZ;//Back Left
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
        if ( Throttle+ThrottleStick+RPMXF<Motor.MinRpm )
        {
            Motor.F=Motor.MinRpm;
        }
        else
        {
            if ( Throttle+ThrottleStick+RPMXF>Motor.MaxRpm )
            {
                Motor.F=Motor.MaxRpm;
            }
            else
            {
                Motor.F=(unsigned short)(Throttle+ThrottleStick+RPMXF); //FRONT
            }
        }
        if ( Throttle+ThrottleStick+RPMXB<Motor.MinRpm )
        {
            Motor.B=Motor.MinRpm;
        }
        else
        {
            if ( Throttle+ThrottleStick+RPMXB>Motor.MaxRpm )
            {
                Motor.B=Motor.MaxRpm;
            }
            else
            {
                Motor.B=(unsigned short)(Throttle+ThrottleStick+RPMXB); //Back
            }
        }
        if ( Throttle+ThrottleStick+RPMYL<Motor.MinRpm )
        {
            Motor.L=Motor.MinRpm;
        }
        else
        {
            if ( Throttle+ThrottleStick+RPMYL>Motor.MaxRpm )
            {
                Motor.L=Motor.MaxRpm;
            }
            else
            {
                Motor.L=(unsigned short)(Throttle+ThrottleStick+RPMYL); //Left
            }
        }
        if ( Throttle+ThrottleStick+RPMYR<Motor.MinRpm )
        {
            Motor.R=Motor.MinRpm;
        }
        else
        {
            if ( Throttle+ThrottleStick+RPMYR>Motor.MaxRpm )
            {
                Motor.R=Motor.MaxRpm;
            }
            else
            {
                Motor.R=(unsigned short)(Throttle+ThrottleStick+RPMYR); //Right
            }
        }
        /* EasyCODE ) */
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        EasyCODE_COMMENT_START EasyCODE ( 0 
        Motor Limit EasyCODE_COMMENT_END
        RPMXF=PIDX-PIDZ;
        RPMXB=(-1*PIDX)-PIDZ;
        EasyCODE_COMMENT_START EasyCODE - EasyCODE_COMMENT_END
        RPMYL=PIDY+PIDZ;
        RPMYR=(-1*PIDY)+PIDZ;
        if ( Throttle+ThrottleStick+RPMXF<Motor.MinRpm )
        {
            Motor.F=Motor.MinRpm;
        }
        else
        {
            if ( Throttle+ThrottleStick+RPMXF>Motor.MaxRpm )
            {
                Motor.F=Motor.MaxRpm;
            }
            else
            {
                Motor.F=(unsigned short)(Throttle+ThrottleStick+RPMXF); //FRONT
            }
        }
        if ( Throttle+ThrottleStick+RPMXB<Motor.MinRpm )
        {
            Motor.B=Motor.MinRpm;
        }
        else
        {
            if ( Throttle+ThrottleStick+RPMXB>Motor.MaxRpm )
            {
                Motor.B=Motor.MaxRpm;
            }
            else
            {
                Motor.B=(unsigned short)(Throttle+ThrottleStick+RPMXB); //Back
            }
        }
        if ( Throttle+ThrottleStick+RPMYL<Motor.MinRpm )
        {
            Motor.L=Motor.MinRpm;
        }
        else
        {
            if ( Throttle+ThrottleStick+RPMYL>Motor.MaxRpm )
            {
                Motor.L=Motor.MaxRpm;
            }
            else
            {
                Motor.L=(unsigned short)(Throttle+ThrottleStick+RPMYL); //Left
            }
        }
        if ( Throttle+ThrottleStick+RPMYR<Motor.MinRpm )
        {
            Motor.R=Motor.MinRpm;
        }
        else
        {
            if ( Throttle+ThrottleStick+RPMYR>Motor.MaxRpm )
            {
                Motor.R=Motor.MaxRpm;
            }
            else
            {
                Motor.R=(unsigned short)(Throttle+ThrottleStick+RPMYR); //Right
            }
        }
        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
}
    /* EasyCODE ( 0 
    Update motors */
    //BLED=1;
    I2C1BRG=44;	//400kHz @ 40Mhz Bus Clock //44
    //I2C2BRG=19;	//800kHz @ 40Mhz Bus Clock //19
    i2c_start1();
    i2c_write1(MOTORF);
    i2c_write1(Motor.F>>8);
    i2c_write1((Motor.F&0x00FF));
    i2c_stop1();
    
    i2c_start1();
    i2c_write1(MOTORB);
    i2c_write1(Motor.B>>8);
    i2c_write1((Motor.B&0x00FF));
    i2c_stop1();
    
    i2c_start1();
    i2c_write1(MOTORL);
    i2c_write1(Motor.L>>8);
    i2c_write1((Motor.L&0x00FF));
    i2c_stop1();
    
    i2c_start1();
    i2c_write1(MOTORR);
    i2c_write1(Motor.R>>8);
    i2c_write1((Motor.R&0x00FF));
    i2c_stop1();
    BLED=0;
    /* EasyCODE ) */
    /* EasyCODE_STRUCTURED_COMMENT_START */
    /*
    EasyCODE_COMMENT_START EasyCODE ( 0 
    Update motors EasyCODE_COMMENT_END
    //BLED=1;
    I2C3BRG=44;	//400kHz @ 40Mhz Bus Clock //44
    //I2C2BRG=19;	//800kHz @ 40Mhz Bus Clock //19
    i2c_start3();
    i2c_write3(MOTORF);
    i2c_write3(Motor.F>>8);
    i2c_write3((Motor.F&0x00FF));
    i2c_stop3();
    
    i2c_start3();
    i2c_write3(MOTORB);
    i2c_write3(Motor.B>>8);
    i2c_write3((Motor.B&0x00FF));
    i2c_stop3();
    
    i2c_start3();
    i2c_write3(MOTORL);
    i2c_write3(Motor.L>>8);
    i2c_write3((Motor.L&0x00FF));
    i2c_stop3();
    
    i2c_start3();
    i2c_write3(MOTORR);
    i2c_write3(Motor.R>>8);
    i2c_write3((Motor.R&0x00FF));
    i2c_stop3();
    BLED=0;
    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
    */
    /* EasyCODE_STRUCTURED_COMMENT_END */
    if ( Mode==2 )
    {
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        if ( AltitudeControlOn==1 )
        {
            EasyCODE_COMMENT_START EasyCODE ( 0 
            Altitude Bode EasyCODE_COMMENT_END
            if ( FreqResponse==1 )
            {
                EasyCODE_COMMENT_START EasyCODE ( 0 
                Measurement EasyCODE_COMMENT_END
                if ( Frequency<EndF )
                {
                    Error=ErrorA;
                    Re_cos_Error+=(Error*cosf(2*PI*Frequency*Time));
                    Im_sin_Error+=(Error*sinf(2*PI*Frequency*Time));
                    
                    Out=((PvalueA)+(IvalueA)+(DvalueA));
                    Re_cos_Out+=(Out*cosf(2*PI*Frequency*Time));
                    Im_sin_Out+=(Out*sinf(2*PI*Frequency*Time));
                    
                    SignalA=Amplitude*sinf(2*PI*Frequency*Time);
                    Re_cos_Signal+=(SignalA*cosf(2*PI*Frequency*Time));
                    Im_sin_Signal+=(SignalA*sinf(2*PI*Frequency*Time));
                    
                    In=((PvalueA)+(IvalueA)+(DvalueA)+(SignalA));
                    Re_cos_In+=(In*cosf(2*PI*Frequency*Time));
                    Im_sin_In+=(In*sinf(2*PI*Frequency*Time));
                    
                    Time+=dt;
                    if ( Time>(N*(1/Frequency)) )
                    {
                        Time=0;
                        SignalA=0;
                        
                        Re_cos_Error_send=Re_cos_Error;
                        Im_sin_Error_send=Im_sin_Error;
                        Re_cos_In_send=Re_cos_In;
                        Im_sin_In_send=Im_sin_In;
                        Re_cos_Out_send=Re_cos_Out;
                        Im_sin_Out_send=Im_sin_Out;
                        Re_cos_Signal_send=Re_cos_Signal;
                        Im_sin_Signal_send=Im_sin_Signal;
                        Finished=1;
                        
                        Re_cos_Error=0;
                        Im_sin_Error=0;
                        Re_cos_In=0;
                        Im_sin_In=0;
                        Re_cos_Out=0;
                        Im_sin_Out=0;
                        Re_cos_Signal=0;
                        Im_sin_Signal=0;
                        if ( Frequency>=25 )
                        {
                            Frequency+=0.5;
                            Amplitude=2;
                            N=100;
                        }
                        else
                        {
                            if ( Frequency>=10 )
                            {
                                Frequency+=0.5;
                                Amplitude=2;
                                N=50;
                            }
                            else
                            {
                                if ( Frequency>6 )
                                {
                                    Frequency+=0.1;
                                    Amplitude=2;
                                    N=25;
                                }
                                else
                                {
                                    if ( Frequency>=4 )
                                    {
                                        Frequency+=0.1;
                                        Amplitude=2;
                                        N=20;
                                    }
                                    else
                                    {
                                        if ( Frequency>=1 )
                                        {
                                            Frequency+=0.1;
                                            Amplitude=2;
                                            N=10;
                                        }
                                        else
                                        {
                                            if ( Frequency>=0.1 )
                                            {
                                                Frequency+=0.1;
                                                Amplitude=2;
                                                N=5;
                                            }
                                            else
                                            {
                                                Frequency=0.1;
                                                Amplitude=2;
                                                N=5;
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
                else
                {
                    Time=0;
                    SignalA=0;
                    FreqResponse=0;
                }
                EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
            }
            else
            {
                Error=ErrorA;
                Out=(PvalueA+IvalueA+DvalueA);
                In=((PvalueA)+(IvalueA)+(DvalueA)+(SignalA));
                Time=0;
                SignalA=0;
                Finished=0;
                Amplitude=2;
                Frequency=StartF;
                N=1;
                
                Re_cos_In=0;
                Im_sin_In=0;
                Re_cos_Out=0;
                Im_sin_Out=0;
            }
            EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
        }
        else
        {
            if ( AltitudeHoldOn==1 )
            {
                EasyCODE_COMMENT_START EasyCODE ( 0 
                Altitude Speed EasyCODE_COMMENT_END
                if ( FreqResponse==1 )
                {
                    EasyCODE_COMMENT_START EasyCODE ( 0 
                    Measurement EasyCODE_COMMENT_END
                    if ( Frequency<EndF )
                    {
                        Error=ErrorAS;
                        Re_cos_Error+=(Error*cosf(2*PI*Frequency*Time));
                        Im_sin_Error+=(Error*sinf(2*PI*Frequency*Time));
                        
                        Out=((PvalueAS)+(IvalueAS)+(DvalueAS));
                        Re_cos_Out+=(Out*cosf(2*PI*Frequency*Time));
                        Im_sin_Out+=(Out*sinf(2*PI*Frequency*Time));
                        
                        SignalAS=Amplitude*sinf(2*PI*Frequency*Time);
                        Re_cos_Signal+=(SignalAS*cosf(2*PI*Frequency*Time));
                        Im_sin_Signal+=(SignalAS*sinf(2*PI*Frequency*Time));
                        
                        In=((PvalueAS)+(IvalueAS)+(DvalueAS)+(SignalAS));
                        Re_cos_In+=(In*cosf(2*PI*Frequency*Time));
                        Im_sin_In+=(In*sinf(2*PI*Frequency*Time));
                        
                        Time+=dt;
                        if ( Time>(N*(1/Frequency)) )
                        {
                            Time=0;
                            SignalAS=0;
                            
                            Re_cos_Error_send=Re_cos_Error;
                            Im_sin_Error_send=Im_sin_Error;
                            Re_cos_In_send=Re_cos_In;
                            Im_sin_In_send=Im_sin_In;
                            Re_cos_Out_send=Re_cos_Out;
                            Im_sin_Out_send=Im_sin_Out;
                            Re_cos_Signal_send=Re_cos_Signal;
                            Im_sin_Signal_send=Im_sin_Signal;
                            Finished=1;
                            
                            Re_cos_Error=0;
                            Im_sin_Error=0;
                            Re_cos_In=0;
                            Im_sin_In=0;
                            Re_cos_Out=0;
                            Im_sin_Out=0;
                            Re_cos_Signal=0;
                            Im_sin_Signal=0;
                            if ( Frequency>=25 )
                            {
                                Frequency+=0.5;
                                Amplitude=250;
                                N=100;
                            }
                            else
                            {
                                if ( Frequency>=10 )
                                {
                                    Frequency+=0.5;
                                    Amplitude=250;
                                    N=50;
                                }
                                else
                                {
                                    if ( Frequency>6 )
                                    {
                                        Frequency+=0.1;
                                        Amplitude=250;
                                        N=25;
                                    }
                                    else
                                    {
                                        if ( Frequency>=4 )
                                        {
                                            Frequency+=0.1;
                                            Amplitude=250;
                                            N=20;
                                        }
                                        else
                                        {
                                            if ( Frequency>=1 )
                                            {
                                                Frequency+=0.1;
                                                Amplitude=250;
                                                N=10;
                                            }
                                            else
                                            {
                                                if ( Frequency>=0.1 )
                                                {
                                                    Frequency+=0.1;
                                                    Amplitude=250;
                                                    N=5;
                                                }
                                                else
                                                {
                                                    Frequency=0.1;
                                                    Amplitude=250;
                                                    N=5;
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                    else
                    {
                        Time=0;
                        SignalA=0;
                        FreqResponse=0;
                    }
                    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                }
                else
                {
                    Error=ErrorAS;
                    Out=(PvalueAS+IvalueAS+DvalueAS);
                    In=((PvalueAS)+(IvalueAS)+(DvalueAS)+(SignalAS));
                    Time=0;
                    SignalAS=0;
                    Finished=0;
                    Amplitude=250;
                    Frequency=StartF;
                    N=1;
                    
                    Re_cos_In=0;
                    Im_sin_In=0;
                    Re_cos_Out=0;
                    Im_sin_Out=0;
                }
                EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
            }
            else
            {
                EasyCODE_COMMENT_START EasyCODE ( 0 
                Cascade Bode EasyCODE_COMMENT_END
                if ( FreqResponse==1 )
                {
                    EasyCODE_COMMENT_START EasyCODE ( 0 
                    Measurement EasyCODE_COMMENT_END
                    if ( Frequency<EndF )
                    {
                        Error=ErrorXS;
                        Re_cos_Error+=(Error*cosf(2*PI*Frequency*Time));
                        Im_sin_Error+=(Error*sinf(2*PI*Frequency*Time));
                        
                        Out=((PvalueXS)+(IvalueXS)+(DvalueXS));
                        Re_cos_Out+=(Out*cosf(2*PI*Frequency*Time));
                        Im_sin_Out+=(Out*sinf(2*PI*Frequency*Time));
                        
                        In=((PvalueXS)+(IvalueXS)+(DvalueXS)+(SignalA));
                        Re_cos_In+=(In*cosf(2*PI*Frequency*Time));
                        Im_sin_In+=(In*sinf(2*PI*Frequency*Time));
                        
                        SignalA=Amplitude*sinf(2*PI*Frequency*Time);
                        Re_cos_Signal+=(SignalA*cosf(2*PI*Frequency*Time));
                        Im_sin_Signal+=(SignalA*sinf(2*PI*Frequency*Time));
                        
                        Time+=dt;
                        if ( Time>(N*(1/Frequency)) )
                        {
                            Time=0;
                            SignalA=0;
                            
                            Re_cos_Error_send=Re_cos_Error;
                            Im_sin_Error_send=Im_sin_Error;
                            Re_cos_In_send=Re_cos_In;
                            Im_sin_In_send=Im_sin_In;
                            Re_cos_Out_send=Re_cos_Out;
                            Im_sin_Out_send=Im_sin_Out;
                            Re_cos_Signal_send=Re_cos_Signal;
                            Im_sin_Signal_send=Im_sin_Signal;
                            Finished=1;
                            
                            Re_cos_Error=0;
                            Im_sin_Error=0;
                            Re_cos_In=0;
                            Im_sin_In=0;
                            Re_cos_Out=0;
                            Im_sin_Out=0;
                            Re_cos_Signal=0;
                            Im_sin_Signal=0;
                            if ( Frequency>=25 )
                            {
                                Frequency+=0.5;
                                Amplitude=10;
                                N=25;
                            }
                            else
                            {
                                if ( Frequency>=10 )
                                {
                                    Frequency+=0.5;
                                    Amplitude=10;
                                    N=25;
                                }
                                else
                                {
                                    if ( Frequency>5 )
                                    {
                                        Frequency+=0.5;
                                        Amplitude=10;
                                        N=20;
                                    }
                                    else
                                    {
                                        if ( Frequency>=1 )
                                        {
                                            Frequency+=0.1;
                                            Amplitude=10;
                                            N=10;
                                        }
                                        else
                                        {
                                            if ( Frequency>=0.1 )
                                            {
                                                Frequency+=0.1;
                                                Amplitude=10;
                                                N=5;
                                            }
                                            else
                                            {
                                                Frequency=0.1;
                                                Amplitude=10;
                                                N=5;
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                    else
                    {
                        Time=0;
                        SignalA=0;
                        FreqResponse=0;
                    }
                    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                }
                else
                {
                    Error=ErrorXS;
                    Out=(PvalueXS+IvalueXS+DvalueXS);
                    In=((PvalueXS)+(IvalueXS)+(DvalueXS)+(SignalA));
                    Time=0;
                    SignalA=0;
                    Finished=0;
                    Amplitude=10;
                    Frequency=StartF;
                    N=3;
                    
                    Re_cos_In=0;
                    Im_sin_In=0;
                    Re_cos_Out=0;
                    Im_sin_Out=0;
                }
                EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                EasyCODE_COMMENT_START EasyCODE ( 0 
                Cascade Bode Z EasyCODE_COMMENT_END
                if ( FreqResponse==1 )
                {
                    EasyCODE_COMMENT_START EasyCODE ( 0 
                    Measurement EasyCODE_COMMENT_END
                    if ( Frequency<EndF )
                    {
                        Error=ErrorZ;
                        Re_cos_Error+=(Error*cosf(2*PI*Frequency*Time));
                        Im_sin_Error+=(Error*sinf(2*PI*Frequency*Time));
                        
                        Out=((PvalueZ)+(IvalueZ)+(DvalueZ));
                        Re_cos_Out+=(Out*cosf(2*PI*Frequency*Time));
                        Im_sin_Out+=(Out*sinf(2*PI*Frequency*Time));
                        
                        In=((PvalueZ)+(IvalueZ)+(DvalueZ)+(SignalZ));
                        Re_cos_In+=(In*cosf(2*PI*Frequency*Time));
                        Im_sin_In+=(In*sinf(2*PI*Frequency*Time));
                        
                        SignalZ=Amplitude*sinf(2*PI*Frequency*Time);
                        Re_cos_Signal+=(SignalZ*cosf(2*PI*Frequency*Time));
                        Im_sin_Signal+=(SignalZ*sinf(2*PI*Frequency*Time));
                        
                        Time+=dt;
                        if ( Time>(N*(1/Frequency)) )
                        {
                            Time=0;
                            SignalZ=0;
                            
                            Re_cos_Error_send=Re_cos_Error;
                            Im_sin_Error_send=Im_sin_Error;
                            Re_cos_In_send=Re_cos_In;
                            Im_sin_In_send=Im_sin_In;
                            Re_cos_Out_send=Re_cos_Out;
                            Im_sin_Out_send=Im_sin_Out;
                            Re_cos_Signal_send=Re_cos_Signal;
                            Im_sin_Signal_send=Im_sin_Signal;
                            Finished=1;
                            
                            Re_cos_Error=0;
                            Im_sin_Error=0;
                            Re_cos_In=0;
                            Im_sin_In=0;
                            Re_cos_Out=0;
                            Im_sin_Out=0;
                            Re_cos_Signal=0;
                            Im_sin_Signal=0;
                            if ( Frequency>=25 )
                            {
                                Frequency+=0.5;
                                Amplitude=250;
                                N=25;
                            }
                            else
                            {
                                if ( Frequency>=10 )
                                {
                                    Frequency+=0.5;
                                    Amplitude=250;
                                    N=25;
                                }
                                else
                                {
                                    if ( Frequency>=5 )
                                    {
                                        Frequency+=0.5;
                                        Amplitude=250;
                                        N=20;
                                    }
                                    else
                                    {
                                        if ( Frequency>=1 )
                                        {
                                            Frequency+=0.5;
                                            Amplitude=250;
                                            N=10;
                                        }
                                        else
                                        {
                                            if ( Frequency>=0.1 )
                                            {
                                                Frequency+=0.1;
                                                Amplitude=250;
                                                N=5;
                                            }
                                            else
                                            {
                                                Frequency=0.1;
                                                Amplitude=250;
                                                N=5;
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                    else
                    {
                        Time=0;
                        SignalZ=0;
                        FreqResponse=0;
                    }
                    EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
                }
                else
                {
                    Error=ErrorZ;
                    Out=((PvalueZ)+(IvalueZ)+(DvalueZ));
                    In=((PvalueZ)+(IvalueZ)+(DvalueZ)+(SignalZ));
                    Time=0;
                    SignalZ=0;
                    Finished=0;
                    Amplitude=250;
                    Frequency=StartF;
                    N=3;
                    
                    Re_cos_In=0;
                    Im_sin_In=0;
                    Re_cos_Out=0;
                    Im_sin_Out=0;
                }
                EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
            }
        }
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
        /* EasyCODE ( 0 
        Cascade Bode */
        if ( FreqResponse==1 )
        {
            /* EasyCODE ( 0 
            Measurement */
            if ( Frequency<EndF )
            {
                Error=ErrorXS;
                Re_cos_Error+=(Error*cosf(2*PI*Frequency*Time));
                Im_sin_Error+=(Error*sinf(2*PI*Frequency*Time));
                
                Out=((PvalueXS)+(IvalueXS)+(DvalueXS));
                Re_cos_Out+=(Out*cosf(2*PI*Frequency*Time));
                Im_sin_Out+=(Out*sinf(2*PI*Frequency*Time));
                
                In=((PvalueXS)+(IvalueXS)+(DvalueXS)+(SignalA));
                Re_cos_In+=(In*cosf(2*PI*Frequency*Time));
                Im_sin_In+=(In*sinf(2*PI*Frequency*Time));
                
                SignalA=Amplitude*sinf(2*PI*Frequency*Time);
                Re_cos_Signal+=(SignalA*cosf(2*PI*Frequency*Time));
                Im_sin_Signal+=(SignalA*sinf(2*PI*Frequency*Time));
                
                Time+=dt;
                if ( Time>(N*(1/Frequency)) )
                {
                    Time=0;
                    SignalA=0;
                    
                    Re_cos_Error_send=Re_cos_Error;
                    Im_sin_Error_send=Im_sin_Error;
                    Re_cos_In_send=Re_cos_In;
                    Im_sin_In_send=Im_sin_In;
                    Re_cos_Out_send=Re_cos_Out;
                    Im_sin_Out_send=Im_sin_Out;
                    Re_cos_Signal_send=Re_cos_Signal;
                    Im_sin_Signal_send=Im_sin_Signal;
                    Finished=1;
                    
                    Re_cos_Error=0;
                    Im_sin_Error=0;
                    Re_cos_In=0;
                    Im_sin_In=0;
                    Re_cos_Out=0;
                    Im_sin_Out=0;
                    Re_cos_Signal=0;
                    Im_sin_Signal=0;
                    if ( Frequency>=25 )
                    {
                        Frequency+=0.5;
                        Amplitude=10;
                        N=25;
                    }
                    else
                    {
                        if ( Frequency>=10 )
                        {
                            Frequency+=0.5;
                            Amplitude=10;
                            N=25;
                        }
                        else
                        {
                            if ( Frequency>5 )
                            {
                                Frequency+=0.5;
                                Amplitude=10;
                                N=20;
                            }
                            else
                            {
                                if ( Frequency>=1 )
                                {
                                    Frequency+=0.1;
                                    Amplitude=10;
                                    N=10;
                                }
                                else
                                {
                                    if ( Frequency>=0.1 )
                                    {
                                        Frequency+=0.1;
                                        Amplitude=10;
                                        N=5;
                                    }
                                    else
                                    {
                                        Frequency=0.1;
                                        Amplitude=10;
                                        N=5;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                Time=0;
                SignalA=0;
                FreqResponse=0;
            }
            /* EasyCODE ) */
        }
        else
        {
            Error=ErrorXS;
            Out=(PvalueXS+IvalueXS+DvalueXS);
            In=((PvalueXS)+(IvalueXS)+(DvalueXS)+(SignalA));
            Time=0;
            SignalA=0;
            Finished=0;
            Amplitude=10;
            Frequency=StartF;
            N=3;
            
            Re_cos_In=0;
            Im_sin_In=0;
            Re_cos_Out=0;
            Im_sin_Out=0;
        }
        /* EasyCODE ) */
        /* EasyCODE_STRUCTURED_COMMENT_START */
        /*
        EasyCODE_COMMENT_START EasyCODE ( 0 
        Acro ModeBode EasyCODE_COMMENT_END
        if ( FreqResponse==1 )
        {
            EasyCODE_COMMENT_START EasyCODE ( 0 
            Measurement EasyCODE_COMMENT_END
            if ( Frequency<=EndF )
            {
                Error=ErrorX;
                Re_cos_Error+=(Error*cosf(2*PI*Frequency*Time));
                Im_sin_Error+=(Error*sinf(2*PI*Frequency*Time));
                
                Out=((PvalueX)+(IvalueX)+(DvalueX));
                Re_cos_Out+=(Out*cosf(2*PI*Frequency*Time));
                Im_sin_Out+=(Out*sinf(2*PI*Frequency*Time));
                
                In=((PvalueX)+(IvalueX)+(DvalueX)+(SignalS));
                Re_cos_In+=(In*cosf(2*PI*Frequency*Time));
                Im_sin_In+=(In*sinf(2*PI*Frequency*Time));
                
                SignalS=Amplitude*sinf(2*PI*Frequency*Time);
                Re_cos_Signal+=(SignalS*cosf(2*PI*Frequency*Time));
                Im_sin_Signal+=(SignalS*sinf(2*PI*Frequency*Time));
                
                Time+=dt;
                if ( Time>(N*(1/Frequency)) )
                {
                    Time=0;
                    SignalS=0;
                    
                    Re_cos_Error_send=Re_cos_Error;
                    Im_sin_Error_send=Im_sin_Error;
                    Re_cos_In_send=Re_cos_In;
                    Im_sin_In_send=Im_sin_In;
                    Re_cos_Out_send=Re_cos_Out;
                    Im_sin_Out_send=Im_sin_Out;
                    Re_cos_Signal_send=Re_cos_Signal;
                    Im_sin_Signal_send=Im_sin_Signal;
                    Finished=1;
                    
                    Re_cos_Error=0;
                    Im_sin_Error=0;
                    Re_cos_In=0;
                    Im_sin_In=0;
                    Re_cos_Out=0;
                    Im_sin_Out=0;
                    Re_cos_Signal=0;
                    Im_sin_Signal=0;
                    if ( Frequency>=25 )
                    {
                        Frequency+=0.5;
                        Amplitude=250;
                        N=10;
                    }
                    else
                    {
                        if ( Frequency>=10 )
                        {
                            Frequency+=0.5;
                            Amplitude=250;
                            N=10;
                        }
                        else
                        {
                            if ( Frequency>6 )
                            {
                                Frequency+=0.1;
                                Amplitude=250;
                                N=10;
                            }
                            else
                            {
                                if ( Frequency>=4 )
                                {
                                    Frequency+=0.1;
                                    Amplitude=250;
                                    N=10;
                                }
                                else
                                {
                                    if ( Frequency>=1 )
                                    {
                                        Frequency+=0.1;
                                        Amplitude=250;
                                        N=5;
                                    }
                                    else
                                    {
                                        if ( Frequency>=0.1 )
                                        {
                                            Frequency+=0.1;
                                            Amplitude=250;
                                            N=5;
                                        }
                                        else
                                        {
                                            Frequency=0.1;
                                            Amplitude=250;
                                            N=5;
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                Time=0;
                SignalS=0;
                FreqResponse=0;
            }
            EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
        }
        else
        {
            Error=ErrorX;
            Out=((PvalueX)+(IvalueX)+(DvalueX));
            In=((PvalueX)+(IvalueX)+(DvalueX)+(SignalS));
            Time=0;
            SignalS=0;
            Finished=0;
            Amplitude=250;
            Frequency=StartF;
            N=1;
            
            Re_cos_In=0;
            Im_sin_In=0;
            Re_cos_Out=0;
            Im_sin_Out=0;
        }
        EasyCODE_COMMENT_START EasyCODE ) EasyCODE_COMMENT_END
        */
        /* EasyCODE_STRUCTURED_COMMENT_END */
}
    else
    {
        /* EasyCODE ( 0 
        Acro ModeBode */
        if ( FreqResponse==1 )
        {
            /* EasyCODE ( 0 
            Measurement */
            if ( Frequency<=EndF )
            {
                Error=(1+(SignalS));
                Re_cos_Error+=(Error*cosf(2*PI*Frequency*Time));
                Im_sin_Error+=(Error*sinf(2*PI*Frequency*Time));
                
                Out=((PvalueX)+(IvalueX)+(DvalueX));
                Re_cos_Out+=(Out*cosf(2*PI*Frequency*Time));
                Im_sin_Out+=(Out*sinf(2*PI*Frequency*Time));
                
                In=(1+(SignalS));
                Re_cos_In+=(In*cosf(2*PI*Frequency*Time));
                Im_sin_In+=(In*sinf(2*PI*Frequency*Time));
                
                SignalS=Amplitude*sinf(2*PI*Frequency*Time);
                Re_cos_Signal+=(SignalS*cosf(2*PI*Frequency*Time));
                Im_sin_Signal+=(SignalS*sinf(2*PI*Frequency*Time));
                
                
                
                Time+=dt;
                if ( Time>(N*(1/Frequency)) )
                {
                    Time=0;
                    SignalS=0;
                    
                    Re_cos_Error_send=Re_cos_Error;
                    Im_sin_Error_send=Im_sin_Error;
                    Re_cos_In_send=Re_cos_In;
                    Im_sin_In_send=Im_sin_In;
                    Re_cos_Out_send=Re_cos_Out;
                    Im_sin_Out_send=Im_sin_Out;
                    Re_cos_Signal_send=Re_cos_Signal;
                    Im_sin_Signal_send=Im_sin_Signal;
                    Finished=1;
                    
                    Re_cos_Error=0;
                    Im_sin_Error=0;
                    Re_cos_In=0;
                    Im_sin_In=0;
                    Re_cos_Out=0;
                    Im_sin_Out=0;
                    Re_cos_Signal=0;
                    Im_sin_Signal=0;
                    if ( Frequency>=25 )
                    {
                        Frequency+=0.5;
                        Amplitude=250;
                        N=10;
                    }
                    else
                    {
                        if ( Frequency>=10 )
                        {
                            Frequency+=0.5;
                            Amplitude=250;
                            N=10;
                        }
                        else
                        {
                            if ( Frequency>6 )
                            {
                                Frequency+=0.1;
                                Amplitude=250;
                                N=10;
                            }
                            else
                            {
                                if ( Frequency>=4 )
                                {
                                    Frequency+=0.1;
                                    Amplitude=250;
                                    N=10;
                                }
                                else
                                {
                                    if ( Frequency>=1 )
                                    {
                                        Frequency+=0.1;
                                        Amplitude=250;
                                        N=5;
                                    }
                                    else
                                    {
                                        if ( Frequency>=0.1 )
                                        {
                                            Frequency+=0.1;
                                            Amplitude=250;
                                            N=5;
                                        }
                                        else
                                        {
                                            Frequency=0.1;
                                            Amplitude=250;
                                            N=5;
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                Time=0;
                SignalS=0;
                FreqResponse=0;
            }
            /* EasyCODE ) */
        }
        else
        {
            Error=ErrorX;
            Out=((PvalueX)+(IvalueX)+(DvalueX));
            In=((PvalueX)+(IvalueX)+(DvalueX)+(SignalS));
            Time=0;
            SignalS=0;
            Finished=0;
            Amplitude=250;
            Frequency=StartF;
            N=1;
            
            Re_cos_In=0;
            Im_sin_In=0;
            Re_cos_Out=0;
            Im_sin_Out=0;
        }
        /* EasyCODE ) */
    }
    /* EasyCODE ) */
    Update=1;
    IFS0bits.T1IF=0;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
RN-41 */
/* EasyCODE F */
void __ISR(_UART_2_VECTOR, IPL7SOFT) IntUart2Handler(void)
{
    UART1Data=U2RXREG;
    if ( UART1Data=='#'
       )
    {
        hekcheck=UART1Data;
        UART1counter=0;
        UART1state=1;
    }
    else
    {
        switch ( UART1state )
        {
        case 0 :
            break;
        case 1 :
            if ( UART1Data==',' )
            {
                UART1Data=0;
                UART1state=2;
            }
            break;
        case 2 :
            Datamode=UART1Data-48;
            UART1state=3;
            break;
        case 3 :
            if ( UART1Data==',' )
            {
                UART1counter=0;
                UART1state=4;
            }
            break;
        case 4 :
            if ( Datamode==1 || Datamode==2 )
            {
                Labview[UART1counter]=UART1Data;
                if ( Labview[48]=='*' )
                {
                    grote=UART1counter;
                    UART1counter=0;
                    UART1state=0;
                    hekcheck=0;
                    RemoteCounter=0;
                    //OLED=0;
                }
                else
                {
                    UART1counter++;
                }
            }
            else
            {
                Labview[UART1counter]=UART1Data;
                if ( Labview[UART1counter]==32 )
                {
                    Labview[UART1counter]=48;
                }
                if ( Labview[UART1counter]=='*' )
                {
                    grote=UART1counter;
                    UART1counter=0;
                    UART1state=0;
                    hekcheck=0;
                    RemoteCounter=0;
                    //OLED=0;
                }
                else
                {
                    UART1counter++;
                }
            }
            break;
        default:
            break;
        }
    }
    if ( U2STAbits.OERR )
    {
        U2STAbits.OERR=0;
    }
    UART1Data=0;
    IFS1bits.U2RXIF=0;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
DSMX */
/* EasyCODE F */
void __ISR(_UART_4_VECTOR, IPL6SOFT) IntUart4Handler(void)
{
    UART2Data=U4RXREG;
    GLED^=1;
    if ( UART2start==1 )
    {
    }
    else
    {
        if ( UART2state==1 )
        {
            DSMX[UART2counter]=UART2Data;
            //UART2counter++;
            if ( UART2counter==10 )
            {
                if ( (DSMX[10]!=0x30) )
                {
                    UART2Data=0;
                    UART2start=0;
                    UART2state=0;
                    DSMX[10]=0;
                }
            }
            if ( UART2counter%2==0 )
            {
                if ( DSMX[UART2counter]==0xA2 )
                {
                    UART2Data=0;
                    UART2start=0;
                    UART2state=0;
                    DSMX[UART2counter]=0;
                    UART2counter=0;
                }
                else
                {
                    UART2counter++;
                }
            }
            else
            {
                UART2counter++;
            }
            if ( UART2counter>13 )
            {
                if ( (DSMX[10]==0x30) && (DSMX[11]==0x00) )
                {
                    UART2start=1;
                    RemoteCounter=0;
                }
                else
                {
                    UART2start=0;
                }
                UART2counter=0;
                UART2state=0;
            }
        }
        else
        {
            if ( UART2Data==0xA2 )
            {
                UART2start=UART2Data;
                UART2state=1;
            }
            else
            {
                GLED=0;
            }
            UART2Data=0;
            UART2counter=0;
        }
    }
    if ( U4STAbits.OERR )
    {
        U4STAbits.OERR=0;
    }
    UART2Data=0;
    /* EasyCODE - */
    IFS2bits.U4RXIF=0;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
GPS */
/* EasyCODE F */
void __ISR(_UART_6_VECTOR, IPL5SOFT) IntUart6Handler(void)
{
    GPSDATA=U6RXREG;
    if ( GPSDATA=='$' && GPS_Start==0 )
    {
        GPS_Start=1;
        GPSCounter=0;
    }
    else
    {
        if ( GPSDATA==0x0D )
        {
            GPS_Start=2;
        }
        else
        {
            if ( GPSDATA==0x0A )
            {
                /* EasyCODE ( 0 
                GGA */
                if ( GPS_Data[3]=='G' && GPS_Data[4]=='G' && GPS_Data[5]=='A' )
                {
                    if ( GPS_Data[GPSCounter-2]<58 )
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-48)*16);
                    }
                    else
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-55)*16);
                    }
                    if ( GPS_Data[GPSCounter-1]<58 )
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-48);
                    }
                    else
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-55);
                    }
                    if ( GPS.CRC==GPSCRC )
                    {
                        memcpy(GPS.GPGGA, GPS_Data, 75);
                    }
                    else
                    {
                        memset(GPS.GPGGA,0,75);
                    }
                }
                /* EasyCODE ) */
                /* EasyCODE ( 0 
                GLL */
                if ( GPS_Data[3]=='G' && GPS_Data[4]=='L' && GPS_Data[5]=='L' )
                {
                    if ( GPS_Data[GPSCounter-2]<58 )
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-48)*16);
                    }
                    else
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-55)*16);
                    }
                    if ( GPS_Data[GPSCounter-1]<58 )
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-48);
                    }
                    else
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-55);
                    }
                    if ( GPS.CRC==GPSCRC )
                    {
                        memcpy(GPS.GPGLL, GPS_Data, 75);
                    }
                    else
                    {
                        memset(GPS.GPGLL,0,75);
                    }
                }
                /* EasyCODE ) */
                /* EasyCODE ( 0 
                RMC */
                if ( GPS_Data[3]=='R' && GPS_Data[4]=='M' && GPS_Data[5]=='C' )
                {
                    if ( GPS_Data[GPSCounter-2]<58 )
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-48)*16);
                    }
                    else
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-55)*16);
                    }
                    if ( GPS_Data[GPSCounter-1]<58 )
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-48);
                    }
                    else
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-55);
                    }
                    if ( GPS.CRC==GPSCRC )
                    {
                        memcpy(GPS.GPRMC, GPS_Data, 75);
                    }
                    else
                    {
                        memset(GPS.GPRMC,0,75);
                    }
                }
                /* EasyCODE ) */
                /* EasyCODE ( 0 
                GSV */
                if ( GPS_Data[3]=='G' && GPS_Data[4]=='S' && GPS_Data[5]=='V' )
                {
                    if ( GPS_Data[GPSCounter-2]<58 )
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-48)*16);
                    }
                    else
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-55)*16);
                    }
                    if ( GPS_Data[GPSCounter-1]<58 )
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-48);
                    }
                    else
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-55);
                    }
                    if ( GPS.CRC==GPSCRC )
                    {
                        memcpy(GPS.GPGSV, GPS_Data, 75);
                    }
                    else
                    {
                        memset(GPS.GPGSV,0,75);
                    }
                }
                /* EasyCODE ) */
                /* EasyCODE ( 0 
                GSA */
                if ( GPS_Data[3]=='G' && GPS_Data[4]=='S' && GPS_Data[5]=='A' )
                {
                    if ( GPS_Data[GPSCounter-2]<58 )
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-48)*16);
                    }
                    else
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-55)*16);
                    }
                    if ( GPS_Data[GPSCounter-1]<58 )
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-48);
                    }
                    else
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-55);
                    }
                    if ( GPS.CRC==GPSCRC )
                    {
                        memcpy(GPS.GPGSA, GPS_Data, 75);
                    }
                    else
                    {
                        memset(GPS.GPGSA,0,75);
                    }
                }
                /* EasyCODE ) */
                /* EasyCODE ( 0 
                VTG */
                if ( GPS_Data[3]=='V' && GPS_Data[4]=='T' && GPS_Data[5]=='G' )
                {
                    if ( GPS_Data[GPSCounter-2]<58 )
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-48)*16);
                    }
                    else
                    {
                        GPSCRC=((GPS_Data[GPSCounter-2]-55)*16);
                    }
                    if ( GPS_Data[GPSCounter-1]<58 )
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-48);
                    }
                    else
                    {
                        GPSCRC+=(GPS_Data[GPSCounter-1]-55);
                    }
                    if ( GPS.CRC==GPSCRC )
                    {
                        memcpy(GPS.GPVTG, GPS_Data, 75);
                    }
                    else
                    {
                        memset(GPS.GPVTG,0,75);
                    }
                }
                /* EasyCODE ) */
                memset(GPS_Data,0,75);
                GPS_Start=0;
                GPSCounter=0;
                GPS.CRC=0;
                GPSCRC=0;
            }
        }
    }
    if ( GPS_Start==1 )
    {
        if ( GPSCounter==0 )
        {
        }
        else
        {
            if ( GPSDATA=='*' )
            {
                GPS.CRC=GPSCRC;
                GPSCRC=0;
            }
            else
            {
                GPSCRC^=GPSDATA;
            }
        }
        GPS_Data[GPSCounter]=GPSDATA;
        GPSCounter++;
    }
    if ( U6STAbits.OERR )
    {
        U6STAbits.OERR=0;
    }
    IFS2bits.U6RXIF=0;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
exception_handler(void) */
/* EasyCODE F */
// This function overrides the normal _weak_ generic handler
void _general_exception_handler(void)
{
    asm volatile("mfc0 %0,$13" : "=r" (_excep_code));
    asm volatile("mfc0 %0,$14" : "=r" (_excep_addr));
    
    _excep_code = (_excep_code & 0x0000007C) >> 2;
    while ( 1 )
    {
        BLED^=1;
        GLED^=1;
        // Examine _excep_code to identify the type of exception
        // Examine _excep_addr to find the address that caused the exception
    }
}

/* EasyCODE ) */
/* EasyCODE ) */
/* EasyCODE ) */
